forked from Telos4/RoboRally
105 lines
2.9 KiB
Python
105 lines
2.9 KiB
Python
import machine
|
|
import time
|
|
|
|
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 control(self, value):
|
|
if value > 0.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(int(abs(value)*1023))
|
|
|
|
|
|
class Robot:
|
|
def __init__(self):
|
|
print("setting up motors")
|
|
self.m1 = Motor(4, 16, 5)
|
|
self.m2 = Motor(14, 0, 2)
|
|
print("setup complete")
|
|
|
|
def forward(self, seconds):
|
|
print("Onward, to glory!")
|
|
self.m1.control(1.0)
|
|
self.m2.control(1.0)
|
|
time.sleep(seconds)
|
|
self.m1.control(0.0)
|
|
self.m2.control(0.0)
|
|
|
|
def backward(self, seconds):
|
|
print("Better retreat...")
|
|
self.m1.control(-1.0)
|
|
self.m2.control(-1.0)
|
|
time.sleep(seconds)
|
|
self.m1.control(0.0)
|
|
self.m2.control(0.0)
|
|
|
|
def spin_right(self, seconds):
|
|
print("You spin my head right round, right round..")
|
|
self.m1.control(1.0)
|
|
self.m2.control(-1.0)
|
|
time.sleep(seconds)
|
|
self.m1.control(0.0)
|
|
self.m2.control(0.0)
|
|
|
|
def spin_left(self, seconds):
|
|
print("spinning left")
|
|
self.m1.control(-1.0)
|
|
self.m2.control(1.0)
|
|
time.sleep(seconds)
|
|
self.m1.control(0.0)
|
|
self.m2.control(0.0)
|
|
|
|
def sequence(self):
|
|
self.spin_left(0.3)
|
|
self.spin_right(0.3)
|
|
self.forward(0.5)
|
|
self.backward(0.5)
|
|
|
|
wall_e = Robot()
|
|
|
|
import usocket
|
|
|
|
# wait for connections on port 1234
|
|
mysocket = usocket.socket()
|
|
myaddr = usocket.getaddrinfo('192.168.4.1', 1234)[0][-1]
|
|
mysocket.bind(myaddr)
|
|
mysocket.listen(1)
|
|
res = mysocket.accept() # this blocks until someone connects to the socket
|
|
comm_socket = res[0]
|
|
|
|
# read data until 'q\n' is sent
|
|
listening = True
|
|
while listening:
|
|
data = comm_socket.readline()
|
|
data_str = data.decode()
|
|
print(data_str)
|
|
if data_str == 'f\n':
|
|
wall_e.forward(0.5)
|
|
elif data_str == 'b\n':
|
|
wall_e.backward(0.5)
|
|
elif data_str == 'l\n':
|
|
wall_e.spin_left(0.5)
|
|
elif data_str == 'r\n':
|
|
wall_e.spin_right(0.5)
|
|
elif data_str == 'q\n':
|
|
listening = False
|