-
Notifications
You must be signed in to change notification settings - Fork 1
/
demoarr.py
57 lines (48 loc) · 1.14 KB
/
demoarr.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
#/usr/bin/env python
import time
import hexapod
from hexapod import HpodSimCtrlParam
import numpy as np
import sys
pod = hexapod.Hexapod()
pod.kneeStrength=4
pod.hipStrength=4
pod.dtKnee = 0.1
pod.dtHip = 0.1
t = np.arange(0,5,0.01)
w = 1
xs = abs(np.sin(2*np.pi*t/5))
pod.setControl(HpodSimCtrlParam.RESETEXP)
pod.send()
pod.setControl(HpodSimCtrlParam.CONTINUE)
pod.send()
pod.setControl(HpodSimCtrlParam.LOAD)
print "Begin Exp"
try:
print "Build Array"
for x in xs[::1]:
for leg in pod.legs:
leg.knee.angle=x*2
leg.hip.yangle=2*x-1
leg.hip.xangle=2*x-1
pod.addParam()
print "Add Param %f" % x
print "Array Built... sending"
pod.sendArray()
pod.clearParamArray()
pod.setControl(HpodSimCtrlParam.RUNEXP)
pod.send()
pod.clearParamArray()
#time.sleep(10)
s = ""
time.sleep(1)
while s!="NO":
pod.setControl(HpodSimCtrlParam.CHKBUSY)
s = pod.send()
s = s[:-1]
time.sleep(1)
print "RECV: %s" % s
print "Sent %d packets" % len(xs)
except (KeyboardInterrupt,):
print "Exit Hexapod Client"
sys.exit(0)