RoboRally/micropython_firmware/main.py

130 lines
4.6 KiB
Python

import machine
import sys
from machine import I2C, Pin
import d1motor
import time
import usocket
import esp
class Motor:
def __init__(self, enable, dir1, dir2):
self.enable_pin = machine.Pin(enable, machine.Pin.OUT)
self.enable_pwm = machine.PWM(self.enable_pin)
self.enable_pwm.freq(500)
self.dir1_pin = machine.Pin(dir1, machine.Pin.OUT)
self.dir2_pin = machine.Pin(dir2, machine.Pin.OUT)
self.direction = 1 # default direction: dir1_pin = HIGH, dir2_pin = LOW
self.reverse()
def reverse(self):
self.direction = not self.direction
self.dir1_pin.value(self.direction)
self.dir2_pin.value(not self.direction)
def speed(self, value):
if value > 0: # forward
if not self.direction: # switch direction if necessary
self.reverse()
else: # backward
if self.direction: # switch direction if necessary
self.reverse()
# apply value as pwm signal
self.enable_pwm.duty(abs(value)*10)
class Robot:
def __init__(self):
i2c = False
if i2c:
print("setting up I2C ...")
#d1 = Pin(5)
#d2 = Pin(4)
#i2c = I2C(scl=d1, sda=d2)
#i2c_scan = 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)
else:
self.m1 = Motor(14, 13, 12)
self.m2 = Motor(5, 0, 4)
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
try:
while listening:
# expected data: '(t, u1, u2)'\n"
# where ui = control for motor i
# ui \in [-1.0, 1.0]
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))
u1 = int(float(l[0])*100)
print("u1 = {}".format(u1))
u2 = int(float(l[1])*100)
print("u2 = {}".format(u2))
self.m1.speed(u1)
self.m2.speed(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:
print("Shutting down ...")
u1 = u2 = 0
self.m1.speed(u1)
self.m2.speed(u2)
listening = False
comm_socket.close()
socket.close()
del comm_socket
del socket
wall_e = Robot()
wall_e.remote_control()