From f3312ade7faf75d2ad10f45b24e2c87dc08fbbcb Mon Sep 17 00:00:00 2001 From: spirkelmann Date: Mon, 15 Jul 2019 23:04:23 +0200 Subject: [PATCH] initial commit for roboflut with a single robot --- keyboard_controller.py | 43 ++++++++++ roboflut.py | 180 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 223 insertions(+) create mode 100644 keyboard_controller.py create mode 100644 roboflut.py diff --git a/keyboard_controller.py b/keyboard_controller.py new file mode 100644 index 0000000..cce35e6 --- /dev/null +++ b/keyboard_controller.py @@ -0,0 +1,43 @@ +import socket +import pygame + +pygame.init() +pygame.display.set_mode((640, 480)) + +rc_socket = socket.socket() +try: + rc_socket.connect(('127.0.0.1', 1337)) # connect to robot +except socket.error: + print("could not connect to socket") + + +while True: + u1 = 0 + u2 = 0 + vmax = 10.0 + events = pygame.event.get() + for event in events: + if event.type == pygame.KEYDOWN: + if event.key == pygame.K_LEFT: + u1 = -vmax / 10.0 + u2 = vmax/ 10.0 + + print("turn left: ({},{})".format(u1, u2)) + elif event.key == pygame.K_RIGHT: + u1 = vmax/ 10.0 + + u2 = -vmax/ 10.0 + + print("turn right: ({},{})".format(u1, u2)) + elif event.key == pygame.K_UP: + u1 = vmax + u2 = vmax + print("forward: ({},{})".format(u1, u2)) + elif event.key == pygame.K_DOWN: + u1 = -vmax + u2 = -vmax + print("backward: ({},{})".format(u1, u2)) + rc_socket.send('({},{})\n'.format(u1, u2)) + elif event.type == pygame.KEYUP: + print("key released, resetting: ({},{})".format(u1, u2)) + rc_socket.send('({},{})\n'.format(u1, u2)) diff --git a/roboflut.py b/roboflut.py new file mode 100644 index 0000000..1c240c4 --- /dev/null +++ b/roboflut.py @@ -0,0 +1,180 @@ +import sys +import socket +import time +import numpy as np + +import pygame + +from threading import Thread, Lock + +import scipy.integrate + +single_robot = True + +class Server: + def __init__(self): + self.ip = '127.0.0.1' + self.recv_port = 1337 # port for receiving data + self.send_port = 1338 # port for sending data + self.recv_addr = socket.getaddrinfo(self.ip, self.recv_port)[0][-1] + + if not single_robot: + self.robots = [] + else: + self.rob = Robot() + + self.receiver = Thread(target=self.remote_control) + self.receiver.start() + + def remote_control(self): + print("setting up socket communication ...") + recv_socket = socket.socket() + recv_socket.bind(self.recv_addr) + while True: + print("waiting for connections on {} ...".format(self.recv_addr)) + recv_socket.listen(2) + (comm_socket, address) = recv_socket.accept() # this blocks until someone connects to the socket + + print("starting thread for handling communication with {}".format(address)) + Thread(target=self.handle_communication, args=(comm_socket, address)).start() + + def handle_communication(self, socket, address): + print("connected!") + listening = True + + if not single_robot: + rob = Robot() + self.robots.append(rob) + + try: + while listening: + # expected data: '(u1, u2)'\n" + # where ui = control for motor i + # ui \in [-1.0, 1.0] + data = socket.recv(4096) + data_str = data.decode() + #print("Data received: {}".format(data_str)) + #print("processing data = {}".format(data_str)) + l = data_str.strip('()\n').split(',') + #print("l = {}".format(l)) + u1 = float(l[0]) + #print("u1 = {}".format(u1)) + u2 = float(l[1]) + #print("u2 = {}".format(u2)) + + if single_robot: + self.rob.set_control(u1, u2) + else: + rob.set_control(u1, u2) + except ValueError: + print("ValueError: Data has wrong format.") + print("Data received: {}".format(data_str)) + except IndexError: + print("IndexError: Data has wrong format.") + print("Data received: {}".format(data_str)) + except Exception as e: + print("Some other error occured") + print("Exception: {}".format(e)) + finally: + socket.close() + if not single_robot: + self.robots.remove(rob) + +class Robot: + def __init__(self): + self.x = np.array([0.0, 0.0, 0.0]) + + self.u1 = 0.0 + self.u2 = 0.0 + + self.r = 0.3 + self.R = 0.5 + self.d = 0.2 + + self.mutex = Lock() + + self.omega_max = 50.0 + + self.integrator = scipy.integrate.ode(self.f) + self.integrator.set_integrator('dopri5') + + self.simulator = Thread(target=self.simulate) + self.simulator.start() + + def set_control(self, u1, u2): + self.mutex.acquire() + try: + self.u1 = u1 + self.u2 = u2 + finally: + self.mutex.release() + + def simulate(self): + t0 = time.time() + t = time.time()-t0 + + while True: + self.mutex.acquire() + try: + self.integrator.set_f_params(self.omega_max * np.array([self.u1, self.u2])) + finally: + self.mutex.release() + self.integrator.set_initial_value(self.x, t) + + dt = time.time() - t0 - t + + self.x = self.integrator.integrate(self.integrator.t + dt) + t = time.time() - t0 + + def f(self, t, x, u): + r = self.r + d = self.d + R = self.R + + theta = x[2] + omegar = u[0] + omegal = u[1] + + dx = np.zeros((3,1)) + dx[0] = (r/2.0 * np.cos(theta) - r * d/(2.0 * R) * np.sin(theta)) * omegar + \ + (r/2.0 * np.cos(theta) + r * d/(2.0 * R) * np.sin(theta)) * omegal + dx[1] = (r/2.0 * np.sin(theta) + r * d/(2.0 * R) * np.cos(theta)) * omegar + \ + (r/2.0 * np.sin(theta) - r * d/(2.0 * R) * np.cos(theta)) * omegal + dx[2] = r/(2.0 * R) * (omegar - omegal) + + return dx + +def main(args): + # wait for connections of socket + s = Server() + + background_colour = (255, 255, 255) + (width, height) = (640, 480) + screen = pygame.display.set_mode((width, height)) + pygame.display.set_caption('Roboflut') + screen.fill(background_colour) + pygame.display.flip() + + running = True + while running: + r = s.rob + #for r in s.robots: + x = int(r.x[0] + width/2.0) + y = int(r.x[1] + height / 2.0) + phi = r.x[2] + phases = ( phi + np.pi) % (2 * np.pi ) - np.pi + + if x < 0 or x > width or y < 0 or y > height: + r.x[0] = 0.0 + r.x[1] = 0.0 + c = int((phases + 2.0 * np.pi) / (4.0 * np.pi) * 255) + #color = (c, c, c) + color = (0,0,0) + + screen.set_at((x, y), color) + + pygame.display.flip() + + +if __name__ == '__main__': + main(sys.argv) \ No newline at end of file