You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
126 lines
2.3 KiB
Python
126 lines
2.3 KiB
Python
import RPi.GPIO as GPIO
|
|
import time
|
|
from AlphaBot2 import AlphaBot2
|
|
|
|
Ab = AlphaBot2()
|
|
|
|
TRIG = 22
|
|
ECHO = 27
|
|
|
|
IR = 17
|
|
PWM = 50
|
|
n=0
|
|
|
|
GPIO.setmode(GPIO.BCM)
|
|
GPIO.setwarnings(False)
|
|
|
|
# IR remote input
|
|
GPIO.setup(IR,GPIO.IN)
|
|
|
|
# Ultrasonic sensor
|
|
GPIO.setup(TRIG,GPIO.OUT,initial=GPIO.LOW)
|
|
GPIO.setup(ECHO,GPIO.IN)
|
|
|
|
def Distance():
|
|
GPIO.output(TRIG,GPIO.HIGH)
|
|
time.sleep(0.000015)
|
|
GPIO.output(TRIG,GPIO.LOW)
|
|
while not GPIO.input(ECHO):
|
|
pass
|
|
t1 = time.time()
|
|
while GPIO.input(ECHO):
|
|
pass
|
|
t2 = time.time()
|
|
return (t2-t1)*34000/2
|
|
|
|
def getkey():
|
|
if GPIO.input(IR) == 0:
|
|
count = 0
|
|
while GPIO.input(IR) == 0 and count < 200: #9ms
|
|
count += 1
|
|
time.sleep(0.00006)
|
|
if(count < 10):
|
|
return;
|
|
count = 0
|
|
while GPIO.input(IR) == 1 and count < 80: #4.5ms
|
|
count += 1
|
|
time.sleep(0.00006)
|
|
|
|
idx = 0
|
|
cnt = 0
|
|
data = [0,0,0,0]
|
|
for i in range(0,32):
|
|
count = 0
|
|
while GPIO.input(IR) == 0 and count < 15: #0.56ms
|
|
count += 1
|
|
time.sleep(0.00006)
|
|
|
|
count = 0
|
|
while GPIO.input(IR) == 1 and count < 40: #0: 0.56mx
|
|
count += 1 #1: 1.69ms
|
|
time.sleep(0.00006)
|
|
|
|
if count > 7:
|
|
data[idx] |= 1<<cnt
|
|
if cnt == 7:
|
|
cnt = 0
|
|
idx += 1
|
|
else:
|
|
cnt += 1
|
|
# print data
|
|
if data[0]+data[1] == 0xFF and data[2]+data[3] == 0xFF: #check
|
|
return data[2]
|
|
else:
|
|
print("repeat")
|
|
return "repeat"
|
|
|
|
|
|
print('IRremote Test Start ...')
|
|
Ab.stop()
|
|
try:
|
|
while True:
|
|
key = getkey()
|
|
if(key != None):
|
|
n = 0
|
|
dist = Distance()
|
|
|
|
if key == 0x18:
|
|
if dist < 20)
|
|
Ab.stop()
|
|
print("obstabcle")
|
|
else
|
|
Ab.forward()
|
|
print("forward")
|
|
if key == 0x08:
|
|
Ab.left()
|
|
print("left")
|
|
if key == 0x1c:
|
|
Ab.stop()
|
|
print("stop")
|
|
if key == 0x5a:
|
|
Ab.right()
|
|
print("right")
|
|
if key == 0x52:
|
|
Ab.backward()
|
|
print("backward")
|
|
if key == 0x15:
|
|
if(PWM + 10 < 101):
|
|
PWM = PWM + 10
|
|
Ab.setPWMA(PWM)
|
|
Ab.setPWMB(PWM)
|
|
print(PWM)
|
|
if key == 0x07:
|
|
if(PWM - 10 > -1):
|
|
PWM = PWM - 10
|
|
Ab.setPWMA(PWM)
|
|
Ab.setPWMB(PWM)
|
|
print(PWM)
|
|
else:
|
|
n += 1
|
|
if n > 20000:
|
|
n = 0
|
|
Ab.stop()
|
|
except KeyboardInterrupt:
|
|
GPIO.cleanup();
|
|
|