-
Notifications
You must be signed in to change notification settings - Fork 0
/
roboclaw_speedaccel.py
executable file
·66 lines (58 loc) · 1.42 KB
/
roboclaw_speedaccel.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
58
59
60
61
62
63
64
65
66
#***Before using this example the motor/controller combination must be
#***tuned and the settings saved to the Roboclaw using IonMotion.
#***The Min and Max Positions must be at least 0 and 50000
import time,sys,signal
import roboclaw_driver.roboclaw_driver as rc
def handler(signal,frame):
rc.ForwardMixed(address,0)
sys.exit(0)
def displayspeed():
enc1 = rc.ReadEncM1(address)
enc2 = rc.ReadEncM2(address)
speed1 = rc.ReadSpeedM1(address)
speed2 = rc.ReadSpeedM2(address)
print("Encoder1:"),
if(enc1[0]==1):
print enc1[1],
print format(enc1[2],'02x'),
else:
print "failed",
print "Encoder2:",
if(enc2[0]==1):
print enc2[1],
print format(enc2[2],'02x'),
else:
print "failed " ,
print "Speed1:",
if(speed1[0]):
print speed1[1],
else:
print "failed",
print("Speed2:"),
if(speed2[0]):
print speed2[1]
else:
print "failed "
rc.Open("/dev/ttyACM0", 115200)
address = 0x80
signal.signal(signal.SIGINT,handler)
version = rc.ReadVersion(address)
if version[0]==False:
print "GETVERSION Failed"
else:
print repr(version[1])
qpps_m1 = 50; qpps_m2 = 50; accel = 100
try:
while(1):
rc.SpeedAccelM1(address,accel,qpps_m1)
rc.SpeedAccelM2(address,accel,-qpps_m2)
for i in range(0,200):
displayspeed()
time.sleep(0.01)
rc.SpeedAccelM1(address,accel,-qpps_m1)
rc.SpeedAccelM2(address,accel,qpps_m2)
for i in range(0,200):
displayspeed()
time.sleep(0.01)
except Exception as e:
print(e)