class MotorController(Process): def __init__(self, host='localhost', port=8000): super(MotorController, self).__init__() self.address = (host, port) self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.motor = Motor() def connect(self): print("Connectin to server at ", self.address[0], " ", self.address[1]) self.client_socket.connect(self.address) def run(self): self.connect() data = self.client_socket.recv(2048) if data.strip() == "start": while True: data = self.client_socket.recv(2048).strip() if data == "stop": self.close() direction = map(int, data.split(',')) print(direction) # code to run motor self.motor.drive(direction) def close(self): self.motor.close() self.client_socket.close() sys.exit()
class CarServer(object): def __init__(self, motor_pins=(24, 25), servo_pin=0, port=2012): self.port = port # The motor and servo for driving self.motor = Motor(*motor_pins) self.servo = Servo(servo_pin) # The most recent coordinates from the accelerameter self.coords = (0, 0, 0) # Whether or not to continue running the server self._run = True self.start() def start(self): """ Initialize and start threads. """ self._server_thread = threading.Thread(target=self._server_worker) self._server_thread.start() self._control_thread = threading.Thread(target=self._control_worker) self._control_thread.start() def stop(self): """ Shut down server and control threads. """ self._run = False def _server_worker(self): HOST = '' # Symbolic name meaning all available interfaces PORT = self.port s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.bind((HOST, PORT)) s.listen(1) conn, addr = s.accept() print 'Connected by', addr while self._run: data = conn.recv(1024) if data: coords = data[1:].split(',') x, y, z = [float(n) for n in coords] self.coords = (x, y, z) conn.sendall(data) conn.close() def _control_worker(self): while self._run: x, y, z = self.coords forward_speed = -y/10 turning_power = (x+10)/20 self.motor.drive(forward_speed) self.servo.set(turning_power)
class GyrosphereAutopilot: def __init__(self, event): self.bt_event = event self.led_light = Led_light(0, 0, 0) self.led_light.setName("Led Light") self.cap = cv2.VideoCapture(-1) sleep(.2) self.motor_r = Motor(6, 5, 13) self.motor_l = Motor(16, 18, 12) def pilot(self): while True: # the event is set if there is no bt connection self.bt_event.wait() spd = 100 target_direction = get_target(self.cap) print("target direction:", target_direction) if 0 < target_direction <= 0.25: # turn right self.motor_l.drive("f", spd) self.motor_r.drive("r", 0) self.led_light.set_color(1, 0, 0) self.led_light.run() elif target_direction >= 0.75: # turn left self.motor_l.drive("r", 0) self.motor_r.drive("f", spd) self.led_light.set_color(0, 1, 0) self.led_light.run() elif target_direction == -1: # turn to find something at a slower pace self.motor_l.drive("r", spd / 2) self.motor_r.drive("f", spd / 2) elif 0.75 > target_direction > 0.25: self.motor_l.drive("f", spd) self.motor_r.drive("f", spd) self.led_light.set_color(0, 0, 1) self.led_light.run()
class PiCar: '''Builds a PiCar class''' def __init__(self): #TODO: impliment some behaviour if no client is found #client = ClientConnection() self.tilt = Servo(pwm_pin=0, center=515, minVal=425, maxVal=650) self.pan = Servo(1, 330, 60, 600) self.wheel = Servo(2, 370, 180, 520) #motor = Motor(4,14,15) self.motor = Motor(17, 27, 18) self.sonar = UltraSounder() self.camera = Camera(sonar=self.sonar, clientAddr=None) #TODO: give car access to leds # mainly so it can do the police car thing def drive_f(self, speed=100): ''' Drive forward If no speed is specified, go full speed ''' self.motor.drive(speed) pass def drive_r(self, speed=100): ''' Drive in reverse If no speed is specified, go full speed ''' self.motor.drive(-speed) pass def turn_l(self, angle=None): ''' Turn wheels to the left If no angle specified, incriment from current position ''' if angle: self.wheel.rotate(angle) else: self.wheel.goto_max() pass def turn_r(self, angle=None): ''' Turn wheels to the right If no angle specified, incriment from current position ''' if angle: self.wheel.rotate(-angle) else: self.wheel.goto_min() pass def look_l(self, angle=None): ''' Pan head to the left If no angle specified, incriment from current position ''' if angle: self.pan.rotate(angle) else: self.pan.goto_max() pass def look_r(self, angle=None): ''' Pan head to the right If no angle specified, incriment from current position ''' if angle: self.pan.rotate(-angle) else: self.pan.goto_min() pass def look_u(self, angle=None): ''' Tilt head up If no angle specified, incriment from current position ''' if angle: self.tilt.rotate(angle) else: self.pan.goto_max() pass def look_d(self, angle=None): ''' Tilt head down If no angle specified, incriment from current position ''' if angle: self.tilt.rotate(-angle) else: self.pan.goto_min() pass def all_stop(self): '''Stop motor''' self.motor.stop() pass def all_ahead(self): '''Move all servos to center position''' self.wheel.goto_center() self.pan.goto_center() self.tilt.goto_center() pass def sonar_scan(self, ): '''Scan sonar from left to right''' pass def __del__(self): '''Close everything nicely so the resources can be used again''' self.camera.close() pass #TODO: impliment code that closes everything nicely def shakedown(self): '''Run through all capabilities''' pass
from motor import Motor 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":
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()