117 lines
2.6 KiB
Python
117 lines
2.6 KiB
Python
from machine import Pin, ADC
|
|
from button import Button
|
|
from time import sleep, time, sleep_us
|
|
|
|
DEAD_ZONE = 0.125
|
|
TIMEOUT = 10
|
|
GAMMA = 1
|
|
DELAY_MIN_US = 500
|
|
DELAY_MAX_US = 100000
|
|
|
|
class Motor:
|
|
_LUT = [
|
|
(1, 0, 0, 0),
|
|
(1, 1, 0, 0),
|
|
(0, 1, 0, 0),
|
|
(0, 1, 1, 0),
|
|
(0, 0, 1, 0),
|
|
(0, 0, 1, 1),
|
|
(0, 0, 0, 1),
|
|
(1, 0, 0, 1),
|
|
]
|
|
|
|
def __init__(self, pins, phase=0):
|
|
self._pins = pins
|
|
self._phase = phase
|
|
|
|
def update_pins(self):
|
|
pattern = self._LUT[self._phase]
|
|
for pin, value in zip(self._pins, pattern):
|
|
pin(value)
|
|
|
|
def step(self, dir):
|
|
dir = 1 if dir > 0 else -1 if dir < 0 else 0
|
|
self._phase = (self._phase + dir) % len(self._LUT)
|
|
self.update_pins()
|
|
|
|
def inc(self):
|
|
self.step(1)
|
|
|
|
def dec(self):
|
|
self.step(-1)
|
|
|
|
@property
|
|
def phase(self):
|
|
return self._phase
|
|
|
|
def off(self):
|
|
for pin in self._pins:
|
|
pin(0)
|
|
|
|
|
|
class Joystick:
|
|
_OFFSET = 30000
|
|
_RANGE = 2**16
|
|
|
|
def __init__(self, adcs):
|
|
self._adcs = adcs
|
|
|
|
def pos(self):
|
|
return [2 * adc.read_u16() / self._RANGE - 1 for adc in self._adcs]
|
|
|
|
|
|
# motor = Motor([Pin(i, Pin.OUT) for i in range (4)])
|
|
motors = [Motor([Pin(2 + i + 4 * j, Pin.OUT) for i in range(4)]) for j in range(3)]
|
|
motor_idx = 3
|
|
joystick = Joystick([ADC(i) for i in range(2)])
|
|
rgb_led = [Pin(20 - i, Pin.OUT) for i in range(3)]
|
|
for led in rgb_led:
|
|
led(0)
|
|
|
|
button = Button(22, thresholds_ms=[1000])
|
|
|
|
|
|
def button_cb(pin_state, threshold):
|
|
global motor_idx
|
|
if pin_state == 0 and threshold > 0:
|
|
motor_idx = (motor_idx + 1) % 4
|
|
for i in range(len(rgb_led)):
|
|
rgb_led[i](1 if i == motor_idx or motor_idx == 3 else 0)
|
|
|
|
|
|
button.callback(button_cb)
|
|
|
|
for i in range(len(rgb_led)):
|
|
rgb_led[i](1 if i == motor_idx or motor_idx == 3 else 0)
|
|
|
|
for motor in motors:
|
|
motor.update_pins()
|
|
|
|
now = time()
|
|
|
|
while True:
|
|
joystick_pos = joystick.pos()[1]
|
|
abs_pos = abs(joystick_pos)
|
|
step_dir =1 if joystick_pos < 0 else -1
|
|
dir = joystick_pos < 0
|
|
|
|
if abs_pos > DEAD_ZONE:
|
|
print(joystick_pos, abs_pos)
|
|
now = time()
|
|
if motor_idx != 3:
|
|
motors[motor_idx].step(step_dir)
|
|
else:
|
|
for motor in motors:
|
|
motor.step(step_dir)
|
|
|
|
normalized = (abs_pos - DEAD_ZONE) / (1-DEAD_ZONE)
|
|
delay_us = DELAY_MIN_US + (DELAY_MAX_US- DELAY_MIN_US) * (1-normalized**GAMMA)
|
|
sleep_us(int(delay_us))
|
|
else:
|
|
sleep(0.01)
|
|
|
|
if time() - now > TIMEOUT:
|
|
for motor in motors:
|
|
motor.off()
|
|
|
|
# sleep(0.0025)
|