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