puppet dance

dance.py

import pigpio
import csv
import operator
from time import sleep

PIN_WEST = 5
PIN_RIGHT = 6
PIN_LEFT = 13
PIN_HEAD = 19

MID_PW = 1500
MAX_PW_DIFF = 900
MAX_ANGLE_DIFF = 90

def angle_to_pulse_width(angle):
    if angle < -MAX_ANGLE_DIFF:
        print("rounded angle.")
        return MID_PW - MAX_PW_DIFF
    elif angle > MAX_ANGLE_DIFF:
        print("rounded angle.")
        return MID_PW + MAX_PW_DIFF
    else:
        dw = MAX_PW_DIFF * (float(angle) / MAX_ANGLE_DIFF)
        return MID_PW + dw

def pulse_width_to_angle(width):
    if width < MID_PW - MAX_PW_DIFF:
        print("rounded width.")
        return -MAX_ANGLE_DIFF
    elif width > MID_PW + MAX_PW_DIFF:
        print("rounded width.")
        return MAX_ANGLE_DIFF
    else:
        angle = MAX_ANGLE_DIFF * float(width - MID_PW) / MAX_PW_DIFF
        return angle


def rotate(pin, target_angle):
    target_width = angle_to_pulse_width(target_angle)
    pi.set_servo_pulsewidth(pin, target_width)

def rotate_all(row):
    rotate(PIN_WEST, row[0])
    rotate(PIN_RIGHT, row[1])
    rotate(PIN_LEFT, row[2])
    rotate(PIN_HEAD, row[3])

pi = pigpio.pi()

initial_angles = [pulse_width_to_angle(MID_PW)] * 4
rotate_all(initial_angles)
sleep(0.5)

with open('motion.csv', 'r') as f:
    reader = csv.reader(f)
    fr = next(reader)
    pr = map(lambda s:float(s), fr) # TODO: string -> float
    print pr
    pt = pr[0] # initial time
    pa = pr[1:] # initial angle list

    rotate_all(pa)
    sleep(0.5)

    for row in reader:
        r = map(lambda s:float(s), row) # TODO: string -> float
        print r
        t = r[0] # time
        a = r[1:] # angle list

        rotate_all(a)

        dt = t - pt
        sleep(dt)

        pt = t

pi.stop()

motion.csv