body.py

from machine import Pin, PWM
from time import sleep

ini_freq = 50
ini_duty = 5500

LE_PIN = 11
LS_PIN = 10
RS_PIN = 20
RE_PIN = 21

# SETUP
LE = PWM(Pin(LE_PIN))
LE.freq(50)
LS = PWM(Pin(LS_PIN))
LS.freq(50)
LH = PWM(Pin(14))
LH.freq(50)
LK = PWM(Pin(15))
LK.freq(50)
RK = PWM(Pin(16))
RK.freq(50)
RH = PWM(Pin(17))
RH.freq(50)
RS = PWM(Pin(RS_PIN))
RS.freq(50)
RE = PWM(Pin(RE_PIN))
RE.freq(50)


# The range is [0.5, 2.5] * 10 ** 6.
def angleToPW(angle):
    angleBetween0And180Degrees = min(180, max(angle, 0))
    sum = 0.5 + angleBetween0And180Degrees / 90
    return int(sum * 10**6)


# @TODO: test this.
def setAngle(joint, angle):
    joint.duty_ns(angleToPW(angle))


def motor_test(joint, min_angle=0, max_angle=180):
    for angle in range(min_angle, max_angle, 5):
        setAngle(joint, angle)
        sleep(0.1)
    for angle in range(max_angle, min_angle, -5):
        setAngle(joint, angle)
        sleep(0.1)


def offset(val):
    return val - 500


def stand45():
    LE.duty_u16(1500)
    LS.duty_u16(4500)
    LH.duty_u16(4500)
    LK.duty_u16(1500)
    RK.duty_u16(8500)
    RH.duty_u16(4500)
    RS.duty_u16(4500)
    RE.duty_u16(8500)


def stand():
    LE.duty_u16(1500)
    LS.duty_u16(offset(5000))
    LH.duty_u16(offset(5000))
    LK.duty_u16(1500)
    RK.duty_u16(8500)
    RH.duty_u16(offset(5000))
    RS.duty_u16(offset(5000))
    RE.duty_u16(8500)


def laydown():
    LE.duty_u16(1500)
    LS.duty_u16(offset(8500))
    LH.duty_u16(offset(1500))
    LK.duty_u16(1500)
    RK.duty_u16(8500)
    RH.duty_u16(offset(8500))
    RS.duty_u16(offset(1500))
    RE.duty_u16(8500)


def fold_back():
    LH.duty_u16(offset(7000))
    LK.duty_u16(7000)
    RH.duty_u16(offset(3000))
    RK.duty_u16(3000)


def set_all(pos):
    LE.duty_u16(pos)
    LS.duty_u16(pos)
    LH.duty_u16(pos)
    LK.duty_u16(pos)
    RK.duty_u16(pos)
    RH.duty_u16(pos)
    RS.duty_u16(pos)
    RE.duty_u16(pos)


def set_body(pos):
    LS.duty_u16(pos)
    LH.duty_u16(pos)
    RH.duty_u16(pos)
    RS.duty_u16(pos)