KNEE_REACH = -25
ANKLE_REACH = 40
HIP_REST = -10
KNEE_REST = -30
ANKLE_REST = 40
#----------------------------
# PACKAGES, HELPERS, DRIVER SETUP
#----------------------------
from time import sleep
if TEST == False:
from comm.pwm import PWM
driver = PWM(0x41)
driver.setPWMFreq(50)
# Note: These should be enums
FRONT_LEFT = 'front left'
FRONT_RIGHT = 'front right'
BACK_LEFT = 'back left'
BACK_RIGHT = 'back right'
if TEST == True:
DELAY = 0.0
def map(z, x, y, a, b):
# x to y is the old range
# a to b is the new range
# z is somewhere in between x and y
'RMH': (28, 240, 390, 1),
'RMK': (27, 230, 514, -1),
'RMA': (26, 150, 620, -1),
'LBH': (6, 315, 465, 1),
'LBK': (8, 206, 498, -1),
'LBA': (7, 150, 657, -1),
'RBH': (25, 320, 480, 1),
'RBK': (24, 185, 490, -1),
'RBA': (23, 210, 645, -1),
'N': (18, 150, 650, 1)
}
driver1 = PWM(0x40)
driver2 = PWM(0x41)
driver1.setPWMFreq(60)
driver2.setPWMFreq(60)
def drive(ch, val):
driver = driver1 if ch < 16 else driver2
ch = ch if ch < 16 else ch - 16
driver.setPWM(ch, 0, val)
def constrain(val, min_val, max_val):
return min(max_val, max(min_val, val))
def remap(old_val, (old_min, old_max), (new_min, new_max)):
new_diff = (new_max - new_min) * (old_val - old_min) / float(