diff --git a/roboflut/keyboard_controller.py b/roboflut/keyboard_controller.py deleted file mode 100644 index cce35e6..0000000 --- a/roboflut/keyboard_controller.py +++ /dev/null @@ -1,43 +0,0 @@ -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/roboflut.py b/roboflut/roboflut.py deleted file mode 100644 index 1c240c4..0000000 --- a/roboflut/roboflut.py +++ /dev/null @@ -1,180 +0,0 @@ -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