# robi_def.py holds all constants/properties for Robi the robot
# by wolf2018
# version date: 6-Apr-2020
#
# this module may not have variables, as it is imported with import *
#
# PWM definitions for servos
# enter / edit all servo and PWM properties here
#
# PIO frequency:    basically defines the smallest step for a servo movement (see example)
# pwm_count:        is calculated from the servo pwm frequency.
#
# Example for standard servos 50Hz PWM and 90 degree movement:
#           @ 800kHz pio_freq. we get 1.25us per clock. The pio_pwm takes 2 clock cycles to execute
#           for 50Hz => 20ms standard servo: max count is calculated to: 20ms / 1.25us / 2 = 8000
#           servo_0 degree:     is 1ms or 400 counts high
#           servo_90 degree:    is 2ms or 800 counts high
#
# as PIO pwm counts backwards from pwm_count to zero servo_min/max position is calculated to:
#   min: pwm_count -400     max: pwm_count-800
#
# smallest step (servo resolution) is 90 degree / (800-400) = 0.22 degree
# in robi.py the servo position is calculated in degrees, using inverse kinematics(IK)
#   then degrees are converted to pwm counts
#
# init_delay is the time between servo initializations to a void all servos initialize at the
#    same time. Use init_delay to limit the inrush current if inrush current is an issue

pio_freq = 800000
pwm_count = 8000
servo_min = pwm_count - 400
servo_max = pwm_count - 800
servo_range = 400
init_delay = 0      # set delay in seconds [0.1] to limit servo current during init

# Definitions of length of leg elements and constraints based on servos with 90 degree movement
leg1 = 5    # hip - knee
leg2 = 5.7  # knee -foot

height_max = leg1 + leg2
height_min = leg2
step_max = leg1
step_min = 0
step_default = 1        # set a comfortable step size
safety_height = 0.7     # foot will be lifted, before step starts
delay_default = 30      # delay default between micro steps, to adjust e.g. for servo speed

# Servo definitions:
# to match with IK servo rotation is [0...90]
# "sm"      state machine to use for servo [0..7]
# "pin"     GPIO pin to use for the servo [GPIO pin number]
# "offset"  offset to adjust for mechanical deviation from servo mid point [degree]
# "max_angle"   mechanical limitation towards the 90 degree point of servo to avoid e.g. mechanical blocking [in degree]
# "min_angle"   mechanical limitation towards the 0 degree point [degree]
# "rot"     defines rotation direction True => increase in angle moves limb/foot forward/up [True or False]
# "neutral" defines the neutral position of the servo [0, 45 degree]
#
L_ankle = {"sm": 0,
        "pin": 0,
        "offset": (45+7.0),
        "max_angle": 72,
        "min_angle": 8,
        "rot": False,
        "name": "left ankle",
        "neutral": 45,
           }


L_knee = {"sm": 1,
        "pin": 1,
        "offset": 0,
        "max_angle": 90,
        "min_angle": 0,
        "rot": True,
        "name": "left knee",
        "neutral": 0,
          }


L_hip = {"sm": 2,
        "pin": 2,
        "offset": (45-3.5),
        "max_angle": 90,
        "min_angle": 0,
        "rot": True,
        "name": "left hip",
        "neutral": -45,
         }


R_hip = {"sm": 3,
        "pin": 3,
        "offset": (45-1.5),
        "max_angle": 90,
        "min_angle": 0,
        "rot": False,
        "name": "right hip",
        "neutral": 45,
         }

# remark changed state machine from 4 to 6 as 4 is appearently used for wlan
R_knee = {"sm": 6,
        "pin": 4,
        "offset": 2,
        "max_angle": 90,
        "min_angle": 0,
        "rot": False,
        "name": "right knee",
        "neutral": 0,
          }


R_ankle = {"sm": 5,
        "pin": 5,
        "offset": (45-0.0),
        "max_angle": 75,
        "min_angle": 12,
        "rot": True,
        "name": "right ankle",
        "neutral": 45,
           }