from sense_hat import SenseHat
import time
import imageprocessing_03 as stair
import MotorController_03 as motor
import cv2
import os
import RPi.GPIO as GPIO

## MOTOR config setting ##

# 모터 상태
STOP = 0
FORWARD = 1
BACKWORD = 2

# 모터 채널
CH1 = 0
CH2 = 1

# PIN 입출력 설정
OUTPUT = 1
INPUT = 0

# PIN 설정
HIGH = 1
LOW = 0

# 실제 핀 정의
#PWM PIN
ENA = 26 #37 pin
ENB = 0 #27 pin

#GPIO PIN
IN1 = 19 #35 pin
IN2 = 13 #33 pin
IN3 = 6 #31 pin
IN4 = 5 #29 pin
SW1 = 17 #13 pin
SW2 = 27 #15 pin
SW3 = 16
SW4 = 20

# 핀 설정 함수
def setPinConfig(EN, INA, INB):
GPIO.setup(EN, GPIO.OUT)
GPIO.setup(INA, GPIO.OUT)
GPIO.setup(INB, GPIO.OUT)
# 100khz 로 PWM 동작 시킴
pwm = GPIO.PWM(EN, 100)
# 우선 PWM 멈춤.
pwm.start(0)
return pwm

# 스위치 핀 함수
def setPinSwitch(SW1, SW2, SW3, SW4) :
GPIO.setwarnings(False)
GPIO.setup(SW1,GPIO.IN,GPIO.PUD_UP)
GPIO.setup(SW2,GPIO.IN,GPIO.PUD_UP)
GPIO.setup(SW3,GPIO.IN,GPIO.PUD_UP)
GPIO.setup(SW4,GPIO.IN,GPIO.PUD_UP)

# 모터 제어 함수
def setMotorContorl(pwm, INA, INB, speed, stat):

#모터 속도 제어 PWM
pwm.ChangeDutyCycle(speed)

if stat == FORWARD:
GPIO.output(INA, HIGH)
GPIO.output(INB, LOW)

#뒤로
elif stat == BACKWORD:
GPIO.output(INA, LOW)
GPIO.output(INB, HIGH)

#정지
elif stat == STOP:
GPIO.output(INA, LOW)
GPIO.output(INB, LOW)
# 모터 제어함수 간단하게 사용하기 위해 한번더 래핑(감쌈)
def setMotor(ch, speed, stat):
if ch == CH1:
#pwmA는 핀 설정 후 pwm 핸들을 리턴 받은 값이다.
setMotorContorl(pwmA, IN1, IN2, speed, stat)
else:
#pwmB는 핀 설정 후 pwm 핸들을 리턴 받은 값이다.
setMotorContorl(pwmB, IN3, IN4, speed, stat)

def RC():
if GPIO.input(SW1) == 0 : #forward

SPEED = 12 #LEFT MOTOR
SPEED1 = 12
setMotor(CH1, SPEED, FORWARD)
setMotor(CH2, SPEED1, BACKWORD)
print(“forward”)

elif GPIO.input(SW2) == 0 : #back

SPEED = 12 #LEFT MOTOR
SPEED1 = 12
setMotor(CH1, SPEED, BACKWORD)
setMotor(CH2, SPEED1, FORWARD)
print(“back”)

elif GPIO.input(SW3) == 0 : #LEFT
SPEED = 1 #LEFT MOTOR
SPEED1 = 15
setMotor(CH1, SPEED, FORWARD)
setMotor(CH2, SPEED1, BACKWORD)
print(“here”)
print(“left”)

elif GPIO.input(SW4) == 0 : #right
SPEED = 15 #LEFT MOTOR
SPEED1 = 1
setMotor(CH1, SPEED, FORWARD)
setMotor(CH2, SPEED1, BACKWORD)
print(“right”)

else :
setMotor(CH1, 0, STOP)
setMotor(CH2, 0, STOP)
print(“stop”)

###########################

def stair_detection():
while True:
print(‘in rc’)
RC() #..
i = 0
while i < 50 :
sense.get_orientation_degrees()['pitch'] i = i + 1

if stair_threshold <= sense.get_orientation_degrees()['pitch']:
print(“up stair”)
setMotor(CH1, 0, STOP)
setMotor(CH2, 0, STOP)
os.system(“sudo service uv4l_raspicam restart”)
os.system(“sudo dd if=/dev/video0 of=image.png bs=11M count=1″)

return stair.stair_detection()

elif sense.get_orientation_degrees()['pitch'] <= -stair_threshold :
print(“down stair”)
setMotor(CH1, 0, STOP)
setMotor(CH2, 0, STOP)
os.system(“sudo service uv4l_raspicam restart”)
# down stair sound

return ‘down’
if __name__ == “__main__” :
os.system(“sudo service uv4l_raspicam restart”)
sense = SenseHat()
stair_threshold = 15 # …
roll_threshold = 10 #

# GPIO 모드 설정
GPIO.setmode(GPIO.BCM)

#모터 핀 설정
#핀 설정후 PWM 핸들 얻어옴
pwmA = setPinConfig(ENA, IN1, IN2)
pwmB = setPinConfig(ENB, IN3, IN4)
setPinSwitch(SW1, SW2, SW3, SW4)

SPEED = 12
SPEED1 = 12

while True:
print(” ##### program start ##### “)

detection = stair_detection()

if detection == True: #up stair

print(‘up stair’)
#up stair sound mp3
while sense.get_orientation_degrees()['pitch'] > 3 :

if sense.get_orientation_degrees()['roll'] > roll_threshold:
setMotor(CH1, 10, FORWARD)
setMotor(CH2, 0, STOP)
print(‘left move’)

elif sense.get_orientation_degrees()['roll'] < -roll_threshold:
setMotor(CH1, 0, STOP)
setMotor(CH2, 10, BACKWORD)
print(‘rigth move’)
elif -roll_threshold < sense.get_orientation_degrees()['roll'] < roll_threshold:
setMotor(CH1, 13, FORWARD)
setMotor(CH2, 13, BACKWORD)
print(‘straight’)

elif detection == ‘down’ :

print(‘down stair’)
while sense.get_orientation_degrees()['pitch'] < -3 :

if sense.get_orientation_degrees()['roll'] > roll_threshold:
setMotor(CH1, 0, STOP)
setMotor(CH2, 13, BACKWORD)
print(‘right move’)

elif sense.get_orientation_degrees()['roll'] < -roll_threshold:
setMotor(CH1, 13, FORWARD)
setMotor(CH2, 0, STOP)
print(‘left move’)
elif -roll_threshold < sense.get_orientation_degrees()['roll'] < roll_threshold:
setMotor(CH1, 10, FORWARD)
setMotor(CH2, 10, BACKWORD)
print(‘straight’)

elif detection == False : #wall or 1 stair

print(“wall”)
setMotor(CH1, 0, STOP)
setMotor(CH2, 0, STOP)

GPIO.cleanup()