/
control.py
78 lines (70 loc) · 2.12 KB
/
control.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
import math
from gopigo import enc_tgt, left, right, fwd, bwd, stop
en_debug=0
## 360 roation is ~64 encoder pulses or 5 deg/pulse
## DPR is the Deg:Pulse Ratio or the # of degrees per
## encoder pulse.
DPR = 360.0/61
WHEEL_RAD = 3.25 # Wheels are ~6.5 cm diameter.
CHASS_WID = 13.5 # Chassis is ~13.5 cm wide.
def left_deg(deg=None):
'''
Turn chassis left by a specified number of degrees.
DPR is the #deg/pulse (Deg:Pulse ratio)
This function sets the encoder to the correct number
of pulses and then invokes left().
'''
if deg is not None:
pulse= int(deg/DPR)
enc_tgt(0,1,pulse)
left()
def right_deg(deg=None):
'''
Turn chassis right by a specified number of degrees.
DPR is the #deg/pulse (Deg:Pulse ratio)
This function sets the encoder to the correct number
of pulses and then invokes right().
'''
if deg is not None:
pulse= int(deg/DPR)
enc_tgt(0,1,pulse)
right()
def fwd_cm(dist=None):
'''
Move chassis fwd by a specified number of cm.
This function sets the encoder to the correct number
of pulses and then invokes fwd().
'''
if dist is not None:
pulse = int(cm2pulse(dist))
enc_tgt(1,1,pulse)
fwd()
def bwd_cm(dist=None):
'''
Move chassis bwd by a specified number of cm.
This function sets the encoder to the correct number
of pulses and then invokes bwd().
'''
if dist is not None:
pulse = int(cm2pulse(dist))
enc_tgt(1,1,pulse)
bwd()
def cm2pulse(dist):
'''
Calculate the number of pulses to move the chassis dist cm.
pulses = dist * [pulses/revolution]/[dist/revolution]
'''
wheel_circ = 2*math.pi*WHEEL_RAD # [cm/rev] cm traveled per revolution of wheel
revs = dist/wheel_circ
PPR = 18 # [p/rev] encoder Pulses Per wheel Revolution
pulses = PPR*revs # [p] encoder pulses required to move dist cm.
if en_debug:
print 'WHEEL_RAD',WHEEL_RAD
print 'revs',revs
print 'pulses',pulses
return pulses
if __name__ == '__main__':
import atexit
#atexit.register(stop)
#left_deg(120)
bwd_cm(50)