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_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))
