RoboRally/micropython_firmware/main.py

import machine
import sys
from machine import I2C, Pin

import d1motor

import time
import usocket
import esp

class Robot:
    def __init__(self):
        print("setting up I2C ...")
        d1 = Pin(5)
        d2 = Pin(4)
        i2c = I2C(scl=d1, sda=d2)
        i2c_scan = i2c.scan()
        if len(i2c_scan) > 0:
            i2c_addr = i2c_scan[0]
            print("i2c scan = {}".format(i2c_addr))
            print("setting up motors ...")
            self.m1 = d1motor.Motor(0, i2c, address=i2c_addr)
            self.m2 = d1motor.Motor(1, i2c, address=i2c_addr)
            self.m1.speed(0)
            self.m2.speed(0)
            print("motor setup complete!")
        else:
            print("error: no i2c interfaces found!")
            sys.exit(1) 

        ip = my_ip[0]
        # setup socket for remote control 
        self.addr = usocket.getaddrinfo(ip, 1234)[0][-1]

    def remote_control(self):
        while True:
            print("setting up socket communication ...")
            socket_setup_complete = False
            while not socket_setup_complete:
                try:
                    print("trying to create socket on address {}".format(self.addr))
                    socket = usocket.socket()
                    socket.bind(self.addr)
                    print("socket setup complete!")
                    socket_setup_complete = True
                except Exception as e:
                    print("could not create socket. error msg: {}\nwaiting 1 sec and retrying...".format(e))
                    time.sleep(1.0)

            print("waiting for connections on {} ...".format(self.addr))
            socket.listen(1)
            res = socket.accept() # this blocks until someone connects to the socket
            comm_socket = res[0]
            print("connected!")
            listening = True
            while listening:
                 # expected data: '(t, u1, u2)'\n"
                 # where ui = control for motor i
                 # ui \in [-1.0, 1.0]
                 try:
                     data = comm_socket.readline()
                     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))
                     t = float(l[0])
                     print("t = {}".format(t))
                     u1 = int(float(l[1])*100)
                     print("u1 = {}".format(u1))
                     u2 = int(float(l[2])*100)
                     print("u2 = {}".format(u2))
                 except ValueError:
                     print("ValueError: Data has wrong format.")
                     print("Data received: {}".format(data_str))
                     print("Shutting down ...")
                     u1 = u2 = 0
                     t = 0.0
                     listening = False
                     comm_socket.close()
                     socket.close()
                     del comm_socket
                     del socket
                     print("disconnected!")
                 except IndexError:
                     print("IndexError: Data has wrong format.")
                     print("Data received: {}".format(data_str))
                     print("Shutting down ...")
                     u1 = u2 = 0
                     t = 0.0
                     listening = False
                     comm_socket.close()
                     socket.close()
                     del comm_socket
                     del socket
                     print("disconnected!")
                 except Exception as e:
                     print("Some other error occured")
                     print("Exception: {}".format(e))
                     print("Shutting down ...")
                     u1 = u2 = 0
                     t = 0.0
                     listening = False
                     comm_socket.close()
                     socket.close()
                     del comm_socket
                     del socket
                     print("disconnected!")
                 finally:    
                     self.m1.speed(u1)
                     self.m2.speed(u2)
                     time.sleep(t)
                     self.m1.speed(0)
                     self.m2.speed(0)

wall_e = Robot()
wall_e.remote_control()