Created
January 29, 2018 18:18
-
-
Save opyapeus/9af59f3294a314185eaa9b470bec5a27 to your computer and use it in GitHub Desktop.
puppet dance
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
0 | 90 | 60 | 60 | -45 | |
---|---|---|---|---|---|
0.1119402985 | 87.09677419 | 60 | 60 | -45 | |
0.223880597 | 84.19354839 | 60 | 60 | -45 | |
0.3358208955 | 81.29032258 | 60 | 60 | -45 | |
0.447761194 | 78.38709677 | 60 | 60 | -45 | |
0.5597014925 | 75.48387097 | 60 | 60 | -45 | |
0.671641791 | 72.58064516 | 60 | 60 | -45 | |
0.7835820896 | 69.67741935 | 60 | 60 | -45 | |
0.8955223881 | 66.77419355 | 60 | 60 | -45 | |
1.007462687 | 63.87096774 | 60 | 60 | -45 | |
1.119402985 | 60.96774194 | 60 | 60 | -45 | |
1.231343284 | 58.06451613 | 60 | 60 | -45 | |
1.343283582 | 55.16129032 | 60 | 60 | -45 | |
1.455223881 | 52.25806452 | 60 | 60 | -45 | |
1.567164179 | 49.35483871 | 60 | 60 | -45 | |
1.679104478 | 46.4516129 | 60 | 60 | -45 | |
1.791044776 | 43.5483871 | 60 | 60 | -45 | |
1.902985075 | 40.64516129 | 60 | 60 | -45 | |
2.014925373 | 37.74193548 | 60 | 60 | -45 | |
2.126865672 | 34.83870968 | 60 | 60 | -45 | |
2.23880597 | 31.93548387 | 60 | 60 | -45 | |
2.350746269 | 29.03225806 | 60 | 60 | -45 | |
2.462686567 | 26.12903226 | 60 | 60 | -45 | |
2.574626866 | 23.22580645 | 60 | 60 | -45 | |
2.686567164 | 20.32258065 | 60 | 60 | -45 | |
2.798507463 | 17.41935484 | 60 | 60 | -45 | |
2.910447761 | 14.51612903 | 60 | 60 | -45 | |
3.02238806 | 11.61290323 | 60 | 60 | -45 | |
3.134328358 | 8.709677419 | 60 | 60 | -45 | |
3.246268657 | 5.806451613 | 60 | 60 | -45 | |
3.358208955 | 2.903225806 | 60 | 60 | -45 | |
3.470149254 | 0 | 60 | 60 | -45 | |
3.582089552 | -2.903225806 | 60 | 60 | -45 | |
3.694029851 | -5.806451613 | 60 | 60 | -45 | |
3.805970149 | -8.709677419 | 60 | 60 | -45 | |
3.917910448 | -11.61290323 | 60 | 60 | -45 | |
4.029850746 | -14.51612903 | 60 | 60 | -45 | |
4.141791045 | -17.41935484 | 60 | 60 | -45 | |
4.253731343 | -20.32258065 | 60 | 60 | -45 | |
4.365671642 | -23.22580645 | 60 | 60 | -45 | |
4.47761194 | -26.12903226 | 60 | 60 | -45 | |
4.589552239 | -29.03225806 | 60 | 60 | -45 | |
4.701492537 | -31.93548387 | 60 | 60 | -45 | |
4.813432836 | -34.83870968 | 60 | 60 | -45 | |
4.925373134 | -37.74193548 | 60 | 60 | -45 | |
5.037313433 | -40.64516129 | 60 | 60 | -45 | |
5.149253731 | -43.5483871 | 60 | 60 | -45 | |
5.26119403 | -46.4516129 | 60 | 60 | -45 | |
5.373134328 | -49.35483871 | 60 | 60 | -45 | |
5.485074627 | -52.25806452 | 60 | 60 | -45 | |
5.597014925 | -55.16129032 | 60 | 60 | -45 | |
5.708955224 | -58.06451613 | 60 | 60 | -45 | |
5.820895522 | -60.96774194 | 60 | 60 | -45 | |
5.932835821 | -63.87096774 | 60 | 60 | -45 | |
6.044776119 | -66.77419355 | 60 | 60 | -45 | |
6.156716418 | -69.67741935 | 60 | 60 | -45 | |
6.268656716 | -72.58064516 | 60 | 60 | -45 | |
6.380597015 | -75.48387097 | 60 | 60 | -45 | |
6.492537313 | -78.38709677 | 60 | 60 | -45 | |
6.604477612 | -81.29032258 | 60 | 60 | -45 | |
6.71641791 | -84.19354839 | 60 | 60 | -45 | |
6.828358209 | -87.09677419 | 60 | 60 | -45 | |
6.940298507 | -90 | 30 | 30 | -45 | |
7.052238806 | -45 | 0 | 0 | -22.5 | |
7.164179104 | 0 | 30 | -30 | 0 | |
7.276119403 | 0 | 60 | -60 | 0 | |
7.388059701 | 0 | 60 | -60 | 0 | |
7.5 | 0 | 60 | -60 | 0 | |
7.611940299 | 0 | 60 | -60 | 0 | |
7.723880597 | 0 | 60 | -60 | 0 | |
7.835820896 | 0 | 60 | -60 | 0 | |
7.947761194 | 0 | 60 | -60 | 0 | |
8.059701493 | 0 | 60 | -60 | 0 | |
8.171641791 | 0 | 0 | -60 | 0 | |
8.28358209 | 0 | -60 | -60 | 0 | |
8.395522388 | 0 | 0 | -60 | 0 | |
8.507462687 | 0 | 60 | -60 | 0 | |
8.619402985 | 0 | 60 | -60 | 0 | |
8.731343284 | 0 | 60 | -60 | 0 | |
8.843283582 | 0 | 60 | -60 | 0 | |
8.955223881 | 0 | 60 | -60 | 0 | |
9.067164179 | 0 | 60 | -60 | 0 | |
9.179104478 | 0 | 60 | -60 | 0 | |
9.291044776 | 0 | 60 | -60 | 0 | |
9.402985075 | 0 | 60 | -60 | 0 | |
9.514925373 | 0 | 60 | -60 | 0 | |
9.626865672 | 0 | 60 | -60 | 0 | |
9.73880597 | 0 | 60 | -60 | 0 | |
9.850746269 | 0 | 60 | -60 | 0 | |
9.962686567 | 0 | 0 | -60 | 0 | |
10.07462687 | 0 | -60 | -60 | 0 | |
10.18656716 | 0 | 0 | -60 | 0 | |
10.29850746 | 0 | 60 | -60 | 0 | |
10.41044776 | 0 | 60 | -60 | 0 | |
10.52238806 | 0 | 60 | -60 | 0 | |
10.63432836 | 0 | 60 | -60 | 0 | |
10.74626866 | 0 | 60 | -60 | 0 | |
10.85820896 | 0 | 60 | -60 | 0 | |
10.97014925 | 0 | 60 | -60 | 0 | |
11.08208955 | 0 | 60 | -60 | 0 | |
11.19402985 | 0 | 60 | -60 | 0 | |
11.30597015 | 0 | 60 | -60 | 0 | |
11.41791045 | 0 | 60 | -60 | 0 | |
11.52985075 | 0 | 60 | -60 | 0 | |
11.64179104 | 0 | 60 | -60 | 0 | |
11.75373134 | 0 | 0 | -60 | 0 | |
11.86567164 | 0 | -60 | -60 | 0 | |
11.97761194 | 0 | 0 | -60 | 0 | |
12.08955224 | 0 | 60 | -60 | 0 | |
12.20149254 | 0 | 60 | -60 | 0 | |
12.31343284 | 0 | 60 | -60 | 0 | |
12.42537313 | 0 | 60 | -60 | 0 | |
12.53731343 | 0 | 60 | -60 | 0 | |
12.64925373 | 0 | 60 | -60 | 0 | |
12.76119403 | 0 | 60 | -60 | 0 | |
12.87313433 | 0 | 60 | -60 | 0 | |
12.98507463 | 0 | 60 | -60 | 0 | |
13.09701493 | 0 | 60 | -60 | 0 | |
13.20895522 | 0 | 60 | -60 | 0 | |
13.32089552 | 0 | 60 | -60 | 0 | |
13.43283582 | 0 | 60 | -60 | 0 | |
13.54477612 | 0 | 60 | -60 | 0 | |
13.65671642 | 0 | 0 | 0 | -7.5 | |
13.76865672 | 0 | -60 | 60 | -15 | |
13.88059701 | 0 | -60 | 60 | -15 | |
13.99253731 | 0 | -52.5 | 52.5 | -15 | |
14.10447761 | 0 | -30 | 30 | -15 | |
14.21641791 | 0 | -30 | 30 | -15 | |
14.32835821 | 7.5 | -45 | 15 | -15 | |
14.44029851 | 0 | -30 | 30 | -15 | |
14.55223881 | 0 | -30 | 30 | -15 | |
14.6641791 | 0 | -30 | 30 | -15 | |
14.7761194 | 7.5 | -45 | 15 | -15 | |
14.8880597 | 0 | -30 | 30 | -15 | |
15 | 0 | -30 | 30 | -15 | |
15.1119403 | 0 | -30 | 30 | -15 | |
15.2238806 | 7.5 | -45 | 15 | -15 | |
15.3358209 | 0 | -30 | 30 | -15 | |
15.44776119 | 0 | -30 | 30 | -15 | |
15.55970149 | 7.5 | -30 | 30 | -15 | |
15.67164179 | 15 | -60 | 0 | -15 | |
15.78358209 | 7.5 | -30 | 30 | -15 | |
15.89552239 | 0 | -30 | 30 | -15 | |
16.00746269 | 0 | -30 | 30 | -15 | |
16.11940299 | -7.5 | -15 | 45 | -15 | |
16.23134328 | 0 | -30 | 30 | -15 | |
16.34328358 | 0 | -30 | 30 | -15 | |
16.45522388 | 0 | -30 | 30 | -15 | |
16.56716418 | -7.5 | -15 | 45 | -15 | |
16.67910448 | 0 | -30 | 30 | -15 | |
16.79104478 | 0 | -30 | 30 | -15 | |
16.90298507 | 0 | -30 | 30 | -15 | |
17.01492537 | -7.5 | -15 | 45 | -15 | |
17.12686567 | 0 | -30 | 30 | -15 | |
17.23880597 | 0 | -30 | 30 | -15 | |
17.35074627 | -7.5 | -30 | 30 | -15 | |
17.46268657 | -15 | 0 | 60 | -15 | |
17.57462687 | -7.5 | -30 | 30 | -15 | |
17.68656716 | 0 | -30 | 30 | -15 | |
17.79850746 | 0 | -30 | 30 | -15 | |
17.91044776 | 7.5 | -45 | 15 | -15 | |
18.02238806 | 0 | -30 | 30 | -15 | |
18.13432836 | 0 | -30 | 30 | -15 | |
18.24626866 | 0 | -30 | 30 | -15 | |
18.35820896 | 7.5 | -45 | 15 | -15 | |
18.47014925 | 0 | -30 | 30 | -15 | |
18.58208955 | 0 | -30 | 30 | -15 | |
18.69402985 | 0 | -30 | 30 | -15 | |
18.80597015 | 7.5 | -45 | 15 | -15 | |
18.91791045 | 0 | -30 | 30 | -15 | |
19.02985075 | 0 | -30 | 30 | -15 | |
19.14179104 | 7.5 | -30 | 30 | -15 | |
19.25373134 | 15 | -60 | 0 | -15 | |
19.36567164 | 7.5 | -30 | 30 | -15 | |
19.47761194 | 0 | -30 | 30 | -15 | |
19.58955224 | 0 | -30 | 30 | -15 | |
19.70149254 | -7.5 | -15 | 45 | -15 | |
19.81343284 | 0 | -30 | 30 | -15 | |
19.92537313 | 0 | -30 | 30 | -15 | |
20.03731343 | 0 | -30 | 30 | -15 | |
20.14925373 | -7.5 | -15 | 45 | -15 | |
20.26119403 | 0 | -30 | 30 | -15 | |
20.37313433 | 0 | -30 | 30 | -15 | |
20.48507463 | 0 | -45 | 30 | -15 | |
20.59701493 | 0 | -60 | 30 | -15 | |
20.70895522 | 0 | -60 | 30 | -15 | |
20.82089552 | 0 | -60 | 30 | -15 | |
20.93283582 | -30 | -60 | 15 | -15 | |
21.04477612 | -60 | -60 | -60 | -15 | |
21.15671642 | -60 | -60 | -60 | -15 | |
21.26865672 | -60 | -60 | -60 | -15 | |
21.38059701 | -60 | -60 | -60 | -15 | |
21.49253731 | -60 | -60 | -30 | -15 | |
21.60447761 | -60 | -60 | -60 | -15 | |
21.71641791 | -60 | -60 | -60 | -15 | |
21.82835821 | -60 | -60 | -60 | -15 | |
21.94029851 | -60 | -60 | -30 | -15 | |
22.05223881 | -60 | -60 | -60 | -15 | |
22.1641791 | -60 | -60 | -60 | -15 | |
22.2761194 | -60 | -60 | -60 | -15 | |
22.3880597 | -60 | -60 | -30 | -15 | |
22.5 | -60 | -60 | -60 | -15 | |
22.6119403 | -60 | -60 | -60 | -15 | |
22.7238806 | -60 | -60 | -60 | -15 | |
22.8358209 | -60 | -60 | -30 | -15 | |
22.94776119 | -60 | -60 | -60 | -15 | |
23.05970149 | -60 | -60 | -60 | -15 | |
23.17164179 | -60 | -30 | -30 | -15 | |
23.28358209 | -30 | -30 | -30 | -15 | |
23.39552239 | -30 | -30 | -30 | -15 | |
23.50746269 | -30 | -30 | -30 | -15 | |
23.61940299 | -30 | -30 | -30 | -15 | |
23.73134328 | 0 | 0 | 0 | -15 | |
23.84328358 | 0 | 0 | 0 | -15 | |
23.95522388 | 0 | 0 | 0 | -15 | |
24.06716418 | 0 | 0 | 0 | -15 | |
24.17910448 | 30 | 30 | 30 | -15 | |
24.29104478 | 30 | 30 | 30 | -15 | |
24.40298507 | 30 | 30 | 30 | -15 | |
24.51492537 | 30 | 30 | 30 | -15 | |
24.62686567 | 60 | 60 | 60 | -15 | |
24.73880597 | 60 | 60 | 60 | -15 | |
24.85074627 | 60 | 60 | 60 | -15 | |
24.96268657 | 60 | 60 | 60 | -15 | |
25.07462687 | 60 | 30 | 60 | -15 | |
25.18656716 | 60 | 60 | 60 | -15 | |
25.29850746 | 60 | 60 | 60 | -15 | |
25.41044776 | 60 | 60 | 60 | -15 | |
25.52238806 | 60 | 30 | 60 | -15 | |
25.63432836 | 60 | 60 | 60 | -15 | |
25.74626866 | 60 | 60 | 60 | -15 | |
25.85820896 | 60 | 60 | 60 | -15 | |
25.97014925 | 60 | 30 | 60 | -15 | |
26.08208955 | 60 | 60 | 60 | -15 | |
26.19402985 | 60 | 60 | 60 | -15 | |
26.30597015 | 60 | 60 | 60 | -15 | |
26.41791045 | 60 | 30 | 60 | -15 | |
26.52985075 | 60 | 60 | 60 | -15 | |
26.64179104 | 60 | 60 | 60 | -15 | |
26.75373134 | 60 | 60 | 30 | -15 | |
26.86567164 | 30 | 60 | 0 | -15 | |
26.97761194 | 30 | 60 | -60 | -15 | |
27.08955224 | 30 | 60 | -60 | -15 | |
27.20149254 | 30 | 60 | -60 | -15 | |
27.31343284 | 0 | 60 | -60 | -15 | |
27.42537313 | 0 | 60 | -60 | -15 | |
27.53731343 | 0 | 60 | -60 | -15 | |
27.64925373 | 0 | 30 | -30 | -15 | |
27.76119403 | 0 | 60 | -60 | -15 | |
27.87313433 | 0 | 60 | -60 | -15 | |
27.98507463 | 0 | 60 | -60 | -15 | |
28.09701493 | 0 | 60 | -60 | -15 | |
28.20895522 | 0 | 60 | -60 | 0 | |
28.32089552 | 0 | 60 | -60 | 0 | |
28.43283582 | 0 | 60 | -60 | 0 | |
28.54477612 | 0 | 60 | -60 | 0 | |
28.65671642 | 0 | 60 | -60 | -15 | |
28.76865672 | 0 | 60 | -60 | 0 | |
28.88059701 | 0 | 60 | -60 | 0 | |
28.99253731 | 0 | 60 | -60 | 0 | |
29.10447761 | 0 | 60 | -60 | -15 | |
29.21641791 | 0 | 60 | -60 | 0 | |
29.32835821 | 0 | 60 | -60 | 0 | |
29.44029851 | 0 | 60 | -60 | 0 | |
29.55223881 | 0 | 60 | -60 | -15 | |
29.6641791 | 0 | 60 | -60 | 0 | |
29.7761194 | 0 | 60 | -60 | 0 | |
29.8880597 | 0 | 60 | -60 | 0 | |
30 | 0 | 60 | -60 | -15 | |
30.1119403 | 0 | 60 | -60 | 0 | |
30.2238806 | 0 | 60 | -60 | 0 | |
30.3358209 | 0 | 60 | -60 | 0 | |
30.44776119 | 0 | 60 | -60 | -15 | |
30.55970149 | 0 | 60 | -60 | 0 | |
30.67164179 | 0 | 60 | -60 | 0 | |
30.78358209 | 0 | 60 | -60 | 0 | |
30.89552239 | 0 | 60 | -60 | 0 | |
31.00746269 | 0 | 60 | -60 | 0 | |
31.11940299 | 0 | 60 | -60 | 0 | |
31.23134328 | 0 | 0 | -60 | 0 | |
31.34328358 | 0 | -60 | -60 | 0 | |
31.45522388 | 0 | 0 | -60 | 0 | |
31.56716418 | 0 | 60 | -60 | 0 | |
31.67910448 | 0 | 60 | 0 | 0 | |
31.79104478 | 0 | 60 | 60 | 0 | |
31.90298507 | 0 | 60 | 0 | 0 | |
32.01492537 | 0 | 60 | -60 | 0 | |
32.12686567 | 0 | 60 | -60 | 0 | |
32.23880597 | 0 | 60 | -60 | 0 | |
32.35074627 | 0 | 60 | -60 | 0 | |
32.46268657 | 0 | 60 | -60 | 0 | |
32.57462687 | 0 | 60 | -60 | 0 | |
32.68656716 | 0 | 60 | -60 | 0 | |
32.79850746 | 0 | 60 | -60 | 0 | |
32.91044776 | 0 | 60 | -60 | 0 | |
33.02238806 | 0 | 0 | -60 | 0 | |
33.13432836 | 0 | -60 | -60 | 0 | |
33.24626866 | 0 | 0 | -60 | 0 | |
33.35820896 | 0 | 60 | -60 | 0 | |
33.47014925 | 0 | 60 | -60 | 0 | |
33.58208955 | 0 | 60 | -60 | -15 | |
33.69402985 | 0 | 60 | -60 | -15 | |
33.80597015 | 0 | 60 | -60 | -15 | |
33.91791045 | 0 | 60 | -60 | -15 | |
34.02985075 | 0 | 0 | 0 | 0 | |
34.14179104 | 0 | 0 | 0 | 0 | |
34.25373134 | 0 | 0 | 0 | 0 | |
34.36567164 | 0 | 0 | 0 | 0 | |
34.47761194 | 30 | -60 | -60 | -30 | |
34.58955224 | 30 | -60 | -60 | -30 | |
34.70149254 | 30 | -60 | -60 | -30 | |
34.81343284 | 30 | -60 | -60 | -30 | |
34.92537313 | 30 | -60 | -60 | -30 |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment