# Simple demo of of the PCA9685 PWM servo/LED controller library.
# This will move channel 0 from min to max position repeatedly.
# Author: Tony DiCola
# License: Public Domain
from __future__ import division
import time
import math
import sys
# Import the PCA9685 module.
import Adafruit_PCA9685
args = sys.argv
# Uncomment to enable debug output.
#import logging
#logging.basicConfig(level=logging.DEBUG)
# Initialise the PCA9685 using the default address (0x40).
pwm = Adafruit_PCA9685.PCA9685()
# Alternatively specify a different address and/or bus:
#pwm = Adafruit_PCA9685.PCA9685(address=0x41, busnum=2)
# Configure min and max servo pulse lengths
servo_min = 130 # Min pulse length out of 4096
servo_max = 640 # Max pulse length out of 4096
offset = [0, 78, 117, 36, 83, 36, 97, 78, 119, 35, 100, 69, 125]
L1=50
L2=56
#rRz = int(args[1])
height = int(args[1])
walkHeight = int(args[2])
stride = int(args[3])
tilt = int(args[4])
period = int(args[5])
# Set frequency to 60hz, good for servos.
pwm.set_pwm_freq(60)
#servo control
def servo_write(ch, ang):
ang = int((servo_max - servo_min) / 180.0 * ang + servo_min)
pwm.set_pwm(16 - ch, 0, ang)
def fRIK(x, th0, z):
zd = z / math.cos(th0/180.0 * math.pi)
ld = math.sqrt(x*x + zd*zd)
phi = math.atan2(x, zd)
th1 = phi - math.acos((L1*L1 + ld*ld - L2*L2)/(2*L1*ld))
th2 = math.acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) + th1
servo_write(1, th0 + offset[1]);
servo_write(2, th1 * 180.0 / math.pi + offset[2]);
servo_write(3, th2 * 180.0 / math.pi + offset[3]);
def rRIK(x, th0, z):
zd = z / math.cos(th0/180.0 * math.pi)
ld = math.sqrt(x*x + zd*zd)
phi = math.atan2(x, zd)
th1 = phi - math.acos((L1*L1 + ld*ld - L2*L2)/(2*L1*ld))
th2 = math.acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) + th1
servo_write(7, th0 + offset[7]);
servo_write(8, th1 * 180.0 / math.pi + offset[8]);
servo_write(9, th2 * 180.0 / math.pi + offset[9]);
def fLIK(x, th0, z):
zd = z / math.cos(th0/180.0 * math.pi)
ld = math.sqrt(x*x + zd*zd)
phi = math.atan2(x, zd)
th1 = phi - math.acos((L1*L1 + ld*ld - L2*L2)/(2*L1*ld))
th2 = math.acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) + th1
servo_write(4, th0 + offset[4]);
servo_write(5, -(th1 * 180.0 / math.pi) + offset[5]);
servo_write(6, -(th2 * 180.0 / math.pi) + offset[6]);
def rLIK(x, th0, z):
zd = z / math.cos(th0/180.0 * math.pi)
ld = math.sqrt(x*x + zd*zd)
phi = math.atan2(x, zd)
th1 = phi - math.acos((L1*L1 + ld*ld - L2*L2)/(2*L1*ld))
th2 = math.acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) + th1
servo_write(10, th0 + offset[10]);
servo_write(11, -(th1 * 180.0 / math.pi) + offset[11]);
servo_write(12, -(th2 * 180.0 / math.pi) + offset[12]);
while True:
time_mSt = time.time() * 1000
tim = 0
while tim < period:
tim = time.time() * 1000 - time_mSt
tt = tim * math.pi / 2.0 / period
fRIK(0, 0, height - walkHeight * math.sin(tt))
rLIK(0, 0, height - walkHeight * math.sin(tt))
time_mSt = time.time() * 1000
tim = 0
while tim < period:
tim = time.time() * 1000 - time_mSt
tt = tim * math.pi / 2.0 / period
fRIK(0, 0, height - walkHeight * math.cos(tt))
rLIK(0, 0, height - walkHeight * math.cos(tt))
time_mSt = time.time() * 1000
tim = 0
while tim < period:
tim = time.time() * 1000 - time_mSt
tt = tim * math.pi / 2.0 / period
rRIK(0, 0, height - walkHeight * math.sin(tt))
fLIK(0, 0, height - walkHeight * math.sin(tt))
time_mSt = time.time() * 1000
tim = 0
while tim < period:
tim = time.time() * 1000 - time_mSt
tt = tim * math.pi / 2.0 / period
rRIK(0, 0, height - walkHeight * math.cos(tt))
fLIK(0, 0, height - walkHeight * math.cos(tt))
