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 AutoPilot(object):
def __init__(self):
self.init_motor()
self.init_camera()
# self.init_infrared()
# self.init_ultrasonic()
self.init_headlight()
self.sens = DotDict({})
def init_motor(self):
self.motor = Motor()
def init_camera(self):
self.camera = Camera()
def init_infrared(self):
self.infrared = Infrared()
def init_headlight(self):
self.headlight = HeadLight()
def init_ultrasonic(self):
self.ultrasonic = Ultrasonic()
def _pilot(self, deviation):
t = 0.1
if -5 <= deviation <= 5:
self.motor.run(t, 20, 20)
elif -10 <= deviation < -5:
self.motor.left_run(t, left_speed=15, right_speed=20)
elif -20 <= deviation < -10:
self.motor.left_run(t, left_speed=5, right_speed=20)
elif deviation < -20:
self.motor.left_run(t, left_speed=1, right_speed=20)
elif 5 < deviation <= 10:
self.motor.right_run(t, left_speed=20, right_speed=15)
elif 10 < deviation <= 20:
self.motor.right_run(t, left_speed=20, right_speed=5)
elif 20 < deviation:
self.motor.right_run(t, left_speed=20, right_speed=1)
self.motor.free()
#time.sleep(0.1)
def _run(self):
debug = 0
counter = 0
dev_cache = []
momentum = 0
dev_m = 0
cache_len = 0
while self.camera.cap.isOpened():
counter += 1
try:
start = time.time()
deviation = self.camera.analyze()
# logging.debug(f'deviation: {deviation}')
# self._pilot(deviation)
if len(dev_cache) == 0:
avg_deviation = deviation
if abs(deviation - avg_deviation) < 20:
dev_cache.append(deviation)
if len(dev_cache) >= 3:
counter = 0
end = time.time()
delta = end - start
logging.debug(f'analyze 3 times: {delta:.3f}s')
avg_deviation = sum(dev_cache) / len(dev_cache)
dev_m = (0.3 * dev_m + avg_deviation) / 1.3
logging.info(f'deviation momentum: {dev_m}')
self._pilot(dev_m)
except KeyboardInterrupt as e:
counter = 0
dev_cache = []
time.sleep(20)
break
def run(self):
try:
logging.debug('Start smartcar in mode auto_pilot with camera ...')
GpioMgmt().init_pwm()
# self.ultrasonic.init_pwm()
self._run()
except KeyboardInterrupt as e:
GpioMgmt().release()
logging.info("[+] Exiting")
raise e
except Exception as e:
GpioMgmt().init_pin()
logging.error(str(e))
class AutoPilot(object):
def __init__(self):
self.init_motor()
self.init_infrared()
self.init_ultrasonic()
self.init_headlight()
self.sens = DotDict({})
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 infrared_detect(self):
left_sensor, right_sensor = self.infrared.detect()
self.sens.infrared = DotDict({
'left': left_sensor,
'right': right_sensor
})
def ultrasonic_detect(self):
front_distance, left_distance, right_distance = self.ultrasonic.detect(
)
self.sens.distance = DotDict({
'front': front_distance,
'left': left_distance,
'right': right_distance
})
def detect(self):
self.ultrasonic_detect()
self.infrared_detect()
def _pilot(self):
block = True
self.detect()
if self.sens.distance.front > 30:
infrared_block = self.infrared_detect_to_turn_car()
if not infrared_block:
block = False
self._run_when_distance_long()
elif 20 <= self.sens.distance.front <= 50:
infrared_block = self.infrared_detect_to_turn_car()
if not infrared_block:
self.motor.right(0.2)
elif self.sens.distance.front < 20:
self.motor.turn_back()
self.motor.free()
return block
def _run_when_distance_long(self, norm=200):
run_time = (self.sens.distance.front - 20) / norm
self.motor.run(run_time)
logging.debug('Run time ^ %f' % run_time)
def infrared_detect_to_turn_car(self):
infrared_block = True
if self.sens.infrared.left is True and self.sens.infrared.right is False:
self.motor.turn_right()
elif self.sens.infrared.left is False and self.sens.infrared.right is True:
self.motor.turn_left()
elif self.sens.infrared.left is False and self.sens.infrared.right is False:
self.motor.turn_back()
else:
infrared_block = False
return infrared_block
def _on_press(self, key):
logging.debug(f'Press {key}')
if key == Key.space:
self.motor.brake()
elif key == Key.esc:
self._pilot()
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)
else:
time.sleep(0.1)
def _on_release(self, key):
if key != Key.ctrl:
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):
self.listen()
def run(self):
try:
logging.debug('Start smartcar in mode auto_pilot with sensors ...')
GpioMgmt().init_pwm()
self.ultrasonic.init_pwm()
self._run()
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