class Infrared(object):
def __init__(self):
super().__init__()
self.motor = Motor()
self.init_sensor()
def init_sensor(self):
GPIO.setup(AvoidSensorLeft, GPIO.IN)
GPIO.setup(AvoidSensorRight, GPIO.IN)
def detect(self):
left_sensor = GPIO.input(AvoidSensorLeft)
right_sensor = GPIO.input(AvoidSensorRight)
return left_sensor, right_sensor
def detect_to_turn_car(self):
"""
遇到障碍物,红外避障模块的指示灯亮,端口电平为LOW
未遇到障碍物,红外避障模块的指示灯灭,端口电平为HIGH
Return:
- Block detected: True
- Block not detected: False
"""
left_sensor, right_sensor = self.detect()
logging.debug(
f'Infra red detecting ... \nleft: {left_sensor} right: {right_sensor}'
)
if left_sensor is True and right_sensor is True:
return False
else:
self.motor.aquire()
if left_sensor is True and right_sensor is False:
self.motor.spin_left(force=True) # 右边探测到有障碍物，有信号返回，原地向左转
time.sleep(0.01)
elif right_sensor is True and left_sensor is False:
self.motor.spin_right(force=True) # 左边探测到有障碍物，有信号返回，原地向右转
time.sleep(0.01)
elif right_sensor is False and left_sensor is False:
self.motor.spin_right(force=True) # 当两侧均检测到障碍物时调用固定方向的避障(原地右转)
time.sleep(0.01)
self.motor.free()
self.motor.release()
return True
class KeyPilot(object):
"""
Control smart car with Keyboard input
"""
def __init__(self):
self.init_motor()
self.init_infrared()
self.init_ultrasonic()
self.init_headlight()
def init_motor(self):
self.motor = Motor()
def init_infrared(self):
self.infrared = Infrared()
def init_headlight(self):
self.headlight = HeadLight()
def init_ultrasonic(self):
self.ultrasonic = Ultrasonic()
def detect_to_turn(self):
self.motor.aquire()
block = self.ultrasonic.detect_to_turn_car()
self.motor.release()
return block
def _on_press(self, key):
logging.debug(f'Press {key}')
block = self.detect_to_turn()
# if self.motor.lock:
# pass
# if block:
# pass
# else:
if key == Key.up:
logging.debug('Run forward ^')
self.motor.run(0.1)
elif key == Key.down:
logging.debug('Run backward _')
self.motor.back(0.1)
elif key == Key.left:
logging.debug('Run left <-')
self.motor.left(0.1)
elif key == Key.right:
logging.debug('Run right ->')
self.motor.right(0.1)
elif key == KeyCode(char='q'):
logging.debug('Spin left <-')
self.motor.spin_left(0.1)
elif key == KeyCode(char='e'):
logging.debug('Spin right ->')
self.motor.spin_right(0.1)
elif key == Key.space:
logging.debug('Brake X')
self.motor.brake(0.1)
else:
time.sleep(0.1)
def _on_release(self, key):
logging.debug(f'Press {key}')
# if self.motor.lock:
# pass
# else:
self.motor.free()
time.sleep(0.1)
def listen(self, *args, **kwargs):
'''
Function to control the bot using threading. Define the clientSocket in the module before use
'''
try:
with Listener(on_press=self._on_press,
on_release=self._on_release) as listener:
listener.join()
except KeyboardInterrupt:
GpioMgmt().release()
self.headlight.off()
logging.info("[+] Exiting")
# clientSocket.close()
raise e
except Exception as e:
GpioMgmt().release()
self.headlight.off()
logging.error(str(e))
# clientSocket.close()
raise e
def run(self):
try:
GpioMgmt().init_pwm()
self.ultrasonic.init_pwm()
self.detect_to_turn()
self.listen()
except Exception as e:
GpioMgmt().init_pin()
logging.error(str(e))
class Ultrasonic(object):
def __init__(self):
super().__init__()
self.pwm_servo = None
self.motor = Motor()
self.init_utltra()
self.init_servo()
self.init_infrared()
self.init_headlight()
# self.init_pwm()
def init_utltra(self):
"""
电机引脚初始化为输出模式
按键引脚初始化为输入模式
超声波引脚初始化
"""
GPIO.setup(EchoPin, GPIO.IN)
GPIO.setup(TrigPin, GPIO.OUT)
def init_servo(self):
"""舵机引脚初始化
"""
GPIO.setup(ServoPin, GPIO.OUT)
def init_pwm(self):
"""
设置舵机的频率和起始占空比
"""
self.pwm_servo = GPIO.PWM(ServoPin, 50)
self.pwm_servo.start(0)
def init_infrared(self):
self.infrared = Infrared()
def init_headlight(self):
self.headlight = HeadLight()
def _detect_distance(self):
"""
超声波函数
TrigPin 输入至少10us的高电平信号
触发超声波模块的测距功能
___|-|____
模块自动发出８个40kHz的超声波脉冲，并自动检测是否有信号返回
_____|-|-|-|-|-|-|-|_____
一旦检测到有回波信号则Echo引脚会输出高电平
高电平持续时间就是超声波从发射到返回的时间
"""
# 触发超声波模块测距功能
GPIO.output(TrigPin, GPIO.HIGH)
time.sleep(0.000015)
GPIO.output(TrigPin, GPIO.LOW)
# 检测回波信号， 一旦检测到有回波信号则Echo引脚会输出高电平
while not GPIO.input(EchoPin):
pass
# start time
t1 = time.time()
# 检测高电平持续时间
while GPIO.input(EchoPin):
pass
# end time
t2 = time.time()
# S = 高电平时间*声速(340m/s) / 2 (m)
# * 100 (cm)
distance = ((t2 - t1) * 340 / 2) * 100
time.sleep(0.01)
return int(distance)
def detect_distance(self):
dist_list = []
for _ in range(5):
dist = self._detect_distance()
if dist >= 500 or dist == 0:
dist = self._detect_distance()
dist_list.append(dist)
distance_list = sorted(dist_list)
distance = sum(distance_list[0:3]) / 3
return distance
def servo_steer(self, pos):
"""
舵机旋转到指定角度
"""
# for i in range(18):
self.pwm_servo.ChangeDutyCycle(2.5 + 10 * pos / 180)
def detect_right_distance(self):
#舵机旋转到0度，即右侧，测距
self.servo_steer(0)
time.sleep(0.8)
right_distance = self.detect_distance()
logging.info("Ultrasonic detecting ... \nRight distance: %d " %
right_distance)
return right_distance
def detect_left_distance(self):
#舵机旋转到180度，即左侧，测距
self.servo_steer(180)
time.sleep(0.8)
left_distance = self.detect_distance()
logging.info("Ultrasonic detecting ... \nLeft distance: %d " %
left_distance)
return left_distance
def detect_front_distance(self):
#舵机旋转到90度，即前方，测距
self.servo_steer(90)
time.sleep(0.8)
front_distance = self.detect_distance()
logging.info("Ultrasonic detecting ... \nFront distance: %d " %
front_distance)
return front_distance
def servo_detect_to_turn_car(self):
"""
舵机旋转超声波测距避障，led根据车的状态显示相应的颜色
"""
self.headlight.green()
right_distance = self.detect_right_distance()
left_distance = self.detect_left_distance()
front_distance = self.detect_front_distance()
self.motor.aquire()
if left_distance < 30 and right_distance < 30 and front_distance < 30:
# 亮品红色，掉头
self.headlight.magenta()
self.motor.spin_right(0.1, left_speed=60, right_speed=60)
time.sleep(0.01)
elif left_distance >= right_distance:
# 亮蓝色, 向左转
self.headlight.yellow()
self.motor.spin_left(0.1, left_speed=60, right_speed=60)
time.sleep(0.01)
elif left_distance <= right_distance:
# 亮品红色，向右转
self.headlight.yellow()
self.motor.spin_right(0.1, left_speed=60, right_speed=60)
time.sleep(0.01)
self.motor.release()
def detect_to_turn_car(self):
block = True
front_distance = self.detect_front_distance()
if front_distance >= 30:
block = self.infrared.detect_to_turn_car()
elif front_distance < 30:
self.servo_detect_to_turn_car()
self.motor.free()
time.sleep(0.1)
return block
def detect(self):
left_distance = None
right_distance = None
front_distance = self.detect_front_distance()
if front_distance < 50:
right_distance = self.detect_right_distance()
left_distance = self.detect_left_distance()
# front_distance = self.detect_front_distance()
self.servo_steer(90)
time.sleep(0.1)
return front_distance, left_distance, right_distance
