Robot code

stw.py

# General reference: https://docs.micropython.org/en/latest/rp2/quickref.html
# Accelerometer library: https://github.com/adafruit/Adafruit_CircuitPython_LIS331

# Units (unless otherwise noted):
#   Length: inches
#   Time:   minute
#   Speed:  inches per minute

from math import pi, sin, sqrt
from collections import namedtuple
from time import time, sleep
from machine import Pin, PWM

### Constants ###############################

DELAY_TIME = 0            # As fast as possible    (in seconds)
DIAMETER = 8
MAX_RPM = 2000            # Not true max, but rather maximum sustainable rpm (cruising)

# idk what pull-up and pull-down are used for, so FIXME

ESC1 = Pin(_____, Pin.OUT)    # idk what this and below should be, so just Pin for now
ESC2 = Pin(_____, Pin.OUT)

ACCELEROMETER_X = Pin(_____, Pin.IN)
ACCELEROMETER_Y = Pin(_____, Pin.IN)
ACCELEROMETER_Z = Pin(_____, Pin.IN)

TRANSMITTER_CH1P = Pin(______, Pin.IN)
TRANSMITTER_CH2P = Pin(______, Pin.IN)
TRANSMITTER_CH3P = Pin(______, Pin.IN)
TRANSMITTER_CH4P = Pin(______, Pin.IN)

# May need to set freq and duty cycle
TRANSMITTER_CH1  = PWM(TRANSMITTER_CH1P)
TRANSMITTER_CH2  = PWM(TRANSMITTER_CH2P)
TRANSMITTER_CH3  = PWM(TRANSMITTER_CH3P)
TRANSMITTER_CH4  = PWM(TRANSMITTER_CH4P)

### Types ###################################

direction = namedtuple('direction', 'mag theta')
accelerometer_data = namedtuple('accelerometer_data', 'x y z')
raw_controller_data = namedtuple('raw_controller_data', 'ch1 ch2 ch3 ch4')    # Replace with actual names

### Global variables ########################

heading = 0

### I/O functions ###########################

def get_accelerometer() -> accelerometer_data:
    ...

def set_raw_motor_speeds(s1: float, s2: float):A
    ...     # Going to have to figure out how to do DShot...

def set_motor_speeds(s1: float, s2: float):    # Uses proper speeds, not electric values
    ...     # Need to use trial and error

def get_raw_controller_data() -> raw_controller_data:
    return raw_controller_data(
        TRANSMITTER_CH1.duty_u16() / 65535,
        TRANSMITTER_CH2.duty_u16() / 65535,
        TRANSMITTER_CH3.duty_u16() / 65535,
        TRANSMITTER_CH4.duty_u16() / 65535
    )

### Logic ###################################

def acceleration_magnitude(acc: accelerometer) -> float:
    return sqrt(acc.x**2 + acc.y**2)      # I don't think there should be any z acceleration

def update_heading(dt: float, acc: accelerometer_data):     # Should work in the limit, but idk how quickly it'll lose precision
    global heading
    a = acceleration_magnitude(acc)
    R = DIAMETER / 2
    s = sqrt(a*R)
    heading += s*dt / R
    # TODO: test function on collisions!!!

def update_motor_speeds(d: direction):
    baseline = MAX_RPM * pi * DIAMETER - d.mag
    m1s =  d.mag * cos(heading - d.theta) + baseline
    m2s = -d.mag * cos(heading - d.theta) + baseline
    set_motor_speeds(m1s, m2s)

### Mainloop ################################

# TODO: replace manual mainloop with built-in scheduling (e.g. machine.Timer or something else)

t = time()
while True:
    dt = t - time()
    update_heading(dt, get_acelerometer())
    t = time()
    d = get_controller_direction()
    update_motor_speeds(d)
    # time.sleep(DELAY_TIME)