-
Notifications
You must be signed in to change notification settings - Fork 0
/
roboclaw_keyenc.py
executable file
·98 lines (84 loc) · 1.89 KB
/
roboclaw_keyenc.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
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
'''
# @brief : This file is a program for PWM control while reading the encoder.
# @author : koji kawabata
# @date : 2019/9/27
'''
import time
import signal
import sys
import roboclaw_driver.roboclaw_driver as rc
from pykbhit import KBHit
def handler(signal,frame):
rc.ForwardMixed(address,0)
rc.TurnRightMixed(address,0)
file.close()
print('Fin')
sys.exit(0)
def displayspeed():
enc1 = rc.ReadEncM1(address)
enc2 = rc.ReadEncM2(address)
speed1 = rc.ReadSpeedM1(address)
speed2 = rc.ReadSpeedM2(address)
list=[str(enc1[1])+",",str(enc2[1])+",",str(speed1[1])+",",str(speed2[1])]
print("Encoder1:"),
if(enc1[0]==1):
print enc1[1],
print format(enc1[2],'02x'),
else:
print "failed",
list[0]="failed"
print "Encoder2:",
if(enc2[0]==1):
print enc2[1],
print format(enc2[2],'02x'),
else:
print "failed " ,
list[1]="failed"
print "Speed1:",
if(speed1[0]):
print speed1[1],
else:
print "failed",
list[2]="failed"
print("Speed2:"),
if(speed2[0]):
print speed2[1]
else:
print "failed"
list[3]="failed"
file.writelines(list)
file.write('\n')
path='encoder-log.csv'
file=open(path,'w')
rc.Open('/dev/ttyACM0',115200)
address = 0x80
pwm=16
rc.ForwardMixed(address, 0)
rc.TurnRightMixed(address, 0)
rc.ResetEncoders(address)
signal.signal(signal.SIGINT,handler)
kb=KBHit()
print('Hit any key(w,a,d,s,e), or Ctr+c to exit')
version = rc.ReadVersion(address)
if version[0] == False:
print "GETVERSION Failed"
else:
print repr(version[1])
while True:
time.sleep(0.010)
displayspeed()
if kb.kbhit():
c=kb.getch()
rc.ForwardMixed(address,0)
rc.TurnRightMixed(address,0)
if ord(c)==119:
rc.ForwardMixed(address, pwm)
elif ord(c)==115:
rc.BackwardMixed(address, pwm)
elif ord(c)==100:
rc.TurnLeftMixed(address, pwm)
elif ord(c)==97:
rc.TurnRightMixed(address, pwm)
elif ord(c)==101:
rc.ForwardMixed(address,0)
rc.TurnRightMixed(address,0)