motor_r = Motor(6, 5, 13)
motor_l = Motor(16, 18, 12)
ctl = "start"
while ctl != "q":
ctl = input('Choose action (f,b,r,tl,tr,s): ')
if ctl == "f" or ctl == "r":
spd = 100 # int(input("speed(0-100):"))
motor_l.drive(ctl, spd)
motor_r.drive(ctl, spd)
if ctl == "b":
spd = 100 # input("force(0-100):")
motor_l.brake(spd)
motor_r.brake(spd)
if ctl == "tl":
spd = 100 # int(input("speed(0-100):"))
motor_l.drive("r", spd)
motor_r.drive("f", spd)
if ctl == "tr":
spd = 100 #int(input("speed(0-100):"))
motor_l.drive("f", spd)
motor_r.drive("r", spd)
if ctl == "q":
motor_l.brake(100)
motor_r.brake(100)
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 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 BluetoothThread(Thread):
def __init__(self, event):
Thread.__init__(self)
self.bt_event = event
# instellen van de motoren op de fysieke pins
self.motor_r = Motor(6, 5, 13)
self.motor_l = Motor(16, 18, 12)
# Standaard bluetooth settings
self.server_sock = BluetoothSocket(RFCOMM)
self.server_sock.bind(("", PORT_ANY))
self.server_sock.listen(1)
# port die het zelfde is op de app
self.uuid = "94f39d29-7d6d-437d-973b-fba39e49d4ee"
# aanbieden van de socket
advertise_service(
self.server_sock,
"Gyrosphere_control_server",
service_id=self.uuid,
service_classes=[self.uuid, SERIAL_PORT_CLASS],
profiles=[SERIAL_PORT_PROFILE],
)
def run(self):
# accept connections forever
while True:
print("Waiting for bluetooth connection")
# blocking, dus we wachten tot een nieuwe verbinding ontstaat
client_sock, client_info = self.server_sock.accept()
# block main autopilot thread
self.bt_event.clear()
print("Accepted connection from ", client_info)
# blink blue light
led_light = Led_light(0, 0, 1)
led_light.run()
try:
# keep accepting commands from connection until connection is broken
while True:
data = client_sock.recv(1024)
if len(data) == 0: break
if len(data) > 1000: break
# print("received [%s]" % data)
# alleen de eerste byte als commandos aan elkaar geplakt zitten
ctl1 = chr(data[0])
ctl = ""
for i in data:
ctl += chr(i)
print(ctl)
spd = 100
if ctl1 == "1":
self.motor_l.drive("f", spd)
self.motor_r.drive("f", spd)
led_light = Led_light(0, 0, 1)
led_light.run()
if ctl1 == "2":
self.motor_l.drive("r", spd)
self.motor_r.drive("r", spd)
if ctl1 == "9":
self.motor_l.brake(spd)
self.motor_r.brake(spd)
if ctl1 == "3":
self.motor_l.drive("r", spd)
self.motor_r.drive("f", spd)
if ctl1 == "4":
self.motor_l.drive("f", spd)
self.motor_r.drive("r", spd)
if ctl == "red":
led_light = Led_light(1, 0, 0)
led_light.run()
if ctl == "yellow":
led_light = Led_light(0, 1, 1)
led_light.run()
if ctl == "green":
led_light = Led_light(0, 1, 0)
led_light.run()
if ctl == "q":
break
except IOError:
pass
print("disconnected")
client_sock.close()
# unblock thread
self.bt_event.set()
