-
Notifications
You must be signed in to change notification settings - Fork 0
/
robohateducar.py
357 lines (290 loc) · 14 KB
/
robohateducar.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
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
from time import sleep, monotonic_ns
import board
import pulseio
class RoboHatEduCar(object):
'''Class for controlling an educational car with stand alone RoboHat MM1 board.
'''
# configurations
DRIVER_PWM_FREQ = 400
'''pwm frequency of the motor driver'''
DUTY_CYCLE_MOTOR_MIN = 15000
'''the lowest duty cycle needed for the motor to keep turning the wheel '''
DUTY_CYCLE_MOTOR_BRAKING = 7000
'''the duty cycle needed against the direction of rotation for the motor to brake '''
DUTY_CYCLE_MOTOR_START = 25000
'''the duty_cycle to be used for the motor to turn the wheel from rest'''
DEFAULT_PEDAL_VALUE = 30
'''default speed in percent 0% - 100%'''
CTRL_P_COEFF = 400
''' P coefficient '''
ENC_DISTANCE_TO_360DEGREES = 77
'''wheel distance in encoder ticks for a 360 degree turn'''
ENC_DISTANCE_TO_1METER = 188
''' wheel distance in encoder ticks for the car to travel 1 meter'''
ENC_DISTANCE_TO_TARGET_RAMPDOWN = 20
'''when the enc_distance to target is lower than this setting we enter ST_RAMPDOWN state'''
ENC_DISTANCE_TO_TARGET_BRAKING = 5 # 7
'''when the enc_distance to target is lower than this setting we enter ST_BRAKING state'''
CTR_EXIT_RAPDOWN = 200
'''max amount of cycles we are allowed to stay in ST_RAMPDOWN state before giving up'''
CTR_EXIT_BRAKING = 20
'''max amount of cycles we are allowed to stay in ST_BRAKING state before giving up'''
# class constants
DIR_BACKWARD = -1
DIR_FORWARD = 1
CMD_TURN_RIGHT = -2
'''Command to turn right'''
CMD_DRV_BACKWARD = -1
'''Command to drive backward'''
CMD_STOP = 0
'''Command to stop'''
CMD_DRV_FORWARD = 1
'''Command to drive forward'''
CMD_TURN_LEFT = 2
'''Command to turn left'''
ST_STARTING = 11
ST_CRUISING = 12
ST_RAMPDOWN = 13
ST_BRAKING = 14
ST_TARGET_REACHED = 15
WHEEL_LEFT = 0
WHEEL_RIGHT = 1
def __init__(self):
# Left Ain1(backward) Ain2(Forward) Right Bin1(backward) Bin2(Forward)
# Ain2
self.driver_left_in2 = pulseio.PWMOut(board.SERVO3, frequency=RoboHatEduCar.DRIVER_PWM_FREQ)
# Ain1
self.driver_left_in1 = pulseio.PWMOut(board.SERVO4, frequency=RoboHatEduCar.DRIVER_PWM_FREQ)
# Bin1
self.driver_right_in1 = pulseio.PWMOut(board.SERVO5, frequency=RoboHatEduCar.DRIVER_PWM_FREQ)
# Bin2
self.driver_right_in2 = pulseio.PWMOut(board.SERVO6, frequency=RoboHatEduCar.DRIVER_PWM_FREQ)
self.wheel_enc_left = pulseio.PulseIn(board.SERVO1, maxlen=30)
self.wheel_enc_right = pulseio.PulseIn(board.SERVO2, maxlen=30)
self.wheel_pos_left = 0
self.wheel_pos_right = 0
self.direction_left_wheel = RoboHatEduCar.DIR_FORWARD
self.direction_right_wheel = RoboHatEduCar.DIR_FORWARD
self.ctr_rampdown = [0, 0]
self.ctr_braking = [0, 0]
self.stop()
"""Drive the car forward or backward by the specified distance.
Argument:
command -- RoboHatEduCar.CMD_DRV_FORWARD or RoboHatEduCar.CMD_DRV_BACKWARD
distance_in_m -- a number (integer or float) in meters
"""
def drive(self, command, distance_in_m):
enc_distance = int(distance_in_m * RoboHatEduCar.ENC_DISTANCE_TO_1METER)
if enc_distance >= 5:
self._drive(command, enc_distance)
else:
print("Distance too small '{}m'".format(distance_in_m))
"""Turn the car left or right by the specified angle in degrees.
Argument:
command -- RoboHatEduCar.CMD_TURN_LEFT or RoboHatEduCar.CMD_TURN_RIGHT
angle -- a number (integer or float) in degrees
"""
def turn(self, command, angle):
enc_distance = int(angle * RoboHatEduCar.ENC_DISTANCE_TO_360DEGREES / 360)
self._drive(command, enc_distance)
"""Stop the car.
"""
def stop(self):
self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_STOP)
self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_STOP)
def _get_wheel_positions(self):
self.wheel_pos_left = self.wheel_pos_left + len(self.wheel_enc_left) * self.direction_left_wheel
self.wheel_enc_left.clear()
self.wheel_pos_right = self.wheel_pos_right + len(self.wheel_enc_right) * self.direction_right_wheel
self.wheel_enc_right.clear()
return (self.wheel_pos_left, self.wheel_pos_right)
def _pedal_to_duty_cycle(self, pedal_value):
if pedal_value <= 0 or pedal_value > 100:
duty_cycle = 0
else:
duty_cycle = int(pedal_value * (65535 - RoboHatEduCar.DUTY_CYCLE_MOTOR_MIN) / 100) + RoboHatEduCar.DUTY_CYCLE_MOTOR_MIN
return duty_cycle
def _drive(self, command, enc_distance):
if (command == RoboHatEduCar.CMD_DRV_FORWARD
or command == RoboHatEduCar.CMD_DRV_BACKWARD
or command == RoboHatEduCar.CMD_TURN_LEFT
or command == RoboHatEduCar.CMD_TURN_RIGHT):
pass
else:
raise ValueError("RoboHatEduCar error, command not defined `{}'.".format(command.__name__))
distance_reached = False
self.ctr_rampdown = [0, 0]
self.ctr_braking = [0, 0]
start_pos_left, start_pos_right = self._get_wheel_positions()
duty_cycle_default = self._pedal_to_duty_cycle(RoboHatEduCar.DEFAULT_PEDAL_VALUE)
print("{} command received with distance: {} start left pos: {}, start right pos: {}".format(
command, enc_distance, start_pos_left, start_pos_right))
while distance_reached is False:
start_time = monotonic_ns()
pos_left, pos_right = self._get_wheel_positions()
abs_distance_left = abs(pos_left - start_pos_left)
abs_distance_right = abs(pos_right - start_pos_right)
error = abs_distance_right - abs_distance_left
ctrl_state_left = self._next_state_logic(RoboHatEduCar.WHEEL_LEFT, abs_distance_left, enc_distance)
ctrl_state_right = self._next_state_logic(RoboHatEduCar.WHEEL_RIGHT, abs_distance_right, enc_distance)
error_left = error * RoboHatEduCar.CTRL_P_COEFF
error_right = - error * RoboHatEduCar.CTRL_P_COEFF
duty_cycle_left = self._output_logic(RoboHatEduCar.WHEEL_LEFT, ctrl_state_left, command, duty_cycle_default, error_left)
duty_cycle_right = self._output_logic(RoboHatEduCar.WHEEL_RIGHT, ctrl_state_right, command, duty_cycle_default, error_right)
if (ctrl_state_left == RoboHatEduCar.ST_TARGET_REACHED and
ctrl_state_right == RoboHatEduCar.ST_TARGET_REACHED):
distance_reached = True
duration = monotonic_ns() - start_time
# print("duration: {}".format(duration))
# print("left pos: {} st: {} dc: {}, right pos: {} st:{} dc:{}".format(
# pos_left, ctrl_state_left, duty_cycle_left, pos_right, ctrl_state_right, duty_cycle_right))
duration = monotonic_ns() - start_time
if duration <= 0.009:
sleep(0.01 - duration/1e9)
# print("left pos: {}, right pos: {}".format(pos_left, pos_right))
def _next_state_logic(self, wheel, curr_distance, target_distance):
distance_to_target = target_distance - curr_distance
if(distance_to_target <= 0):
state = RoboHatEduCar.ST_TARGET_REACHED
elif(curr_distance <= 0):
state = RoboHatEduCar.ST_STARTING
elif(distance_to_target > RoboHatEduCar.ENC_DISTANCE_TO_TARGET_RAMPDOWN):
state = RoboHatEduCar.ST_CRUISING
elif(distance_to_target > RoboHatEduCar.ENC_DISTANCE_TO_TARGET_BRAKING):
if self.ctr_rampdown[wheel] < RoboHatEduCar.CTR_EXIT_RAPDOWN:
state = RoboHatEduCar.ST_RAMPDOWN
self.ctr_rampdown[wheel] += 1
else:
state = RoboHatEduCar.ST_TARGET_REACHED
else:
if self.ctr_braking[wheel] < RoboHatEduCar.CTR_EXIT_BRAKING:
state = RoboHatEduCar.ST_BRAKING
self.ctr_braking[wheel] += 1
else:
state = RoboHatEduCar.ST_TARGET_REACHED
return state
def _output_logic(self, wheel, ctrl_state, command, duty_cycle_cruising, error):
braking = False
if(ctrl_state == RoboHatEduCar.ST_STARTING):
duty_cycle = RoboHatEduCar.DUTY_CYCLE_MOTOR_START
elif(ctrl_state == RoboHatEduCar.ST_RAMPDOWN):
duty_cycle = RoboHatEduCar.DUTY_CYCLE_MOTOR_MIN + error
elif(ctrl_state == RoboHatEduCar.ST_BRAKING):
command = -command # reverse
braking = True
duty_cycle = RoboHatEduCar.DUTY_CYCLE_MOTOR_BRAKING
elif(ctrl_state == RoboHatEduCar.ST_TARGET_REACHED):
duty_cycle = 0
command = RoboHatEduCar.CMD_STOP
elif(ctrl_state == RoboHatEduCar.ST_CRUISING):
duty_cycle = duty_cycle_cruising + error
else:
raise ValueError("RoboHatEduCar error, state not defined `{}'.".format(ctrl_state))
self._drv_wheel(wheel, command, duty_cycle=duty_cycle, braking=braking)
return duty_cycle
def _drv_wheel(self, wheel, command, duty_cycle=None, pedal_value=DEFAULT_PEDAL_VALUE, braking=False):
if duty_cycle is None:
duty_cycle = self._pedal_to_duty_cycle(pedal_value)
if(wheel == RoboHatEduCar.WHEEL_LEFT):
driver_in1 = self.driver_left_in1
driver_in2 = self.driver_left_in2
direction_attr_name = 'direction_left_wheel'
elif(wheel == RoboHatEduCar.WHEEL_RIGHT):
driver_in1 = self.driver_right_in1
driver_in2 = self.driver_right_in2
direction_attr_name = 'direction_right_wheel'
else:
raise ValueError("RoboHatEduCar error, wheel not defined `{}'.".format(wheel))
if (command == RoboHatEduCar.CMD_DRV_FORWARD
or (wheel == RoboHatEduCar.WHEEL_LEFT and command == RoboHatEduCar.CMD_TURN_RIGHT)
or (wheel == RoboHatEduCar.WHEEL_RIGHT and command == RoboHatEduCar.CMD_TURN_LEFT)):
driver_in1.duty_cycle = 0
driver_in2.duty_cycle = duty_cycle
if not braking:
setattr(self, direction_attr_name, RoboHatEduCar.DIR_FORWARD)
elif (command == RoboHatEduCar.CMD_DRV_BACKWARD
or (wheel == RoboHatEduCar.WHEEL_LEFT and command == RoboHatEduCar.CMD_TURN_LEFT)
or (wheel == RoboHatEduCar.WHEEL_RIGHT and command == RoboHatEduCar.CMD_TURN_RIGHT)):
driver_in1.duty_cycle = duty_cycle
driver_in2.duty_cycle = 0
if not braking:
setattr(self, direction_attr_name, RoboHatEduCar.DIR_BACKWARD)
elif command == RoboHatEduCar.CMD_STOP:
driver_in1.duty_cycle = 0
driver_in2.duty_cycle = 0
else:
raise ValueError("RoboHatEduCar error, command not defined `{}'.".format(command))
"""Drive the car according to a test pattern.
"""
def drv_testPattern(self):
print("Test pattern")
self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_DRV_FORWARD)
self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_STOP)
sleep(1)
self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_DRV_FORWARD)
self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_STOP)
sleep(1)
self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_DRV_BACKWARD)
self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_STOP)
sleep(1)
self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_DRV_BACKWARD)
self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_STOP)
sleep(1)
self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_DRV_BACKWARD)
self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_DRV_FORWARD)
sleep(1)
self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_DRV_FORWARD)
self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_DRV_BACKWARD)
sleep(1)
self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_DRV_FORWARD)
self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_DRV_FORWARD)
sleep(1)
self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_DRV_BACKWARD)
self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_DRV_BACKWARD)
sleep(1)
self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_STOP)
self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_STOP)
class EduCarTurtle(object):
def __init__(self, car):
self.car = car
# def position():
def forward(self, distance):
"""Move the turtle forward by the specified distance.
Aliases: forward | fd
Argument:
distance -- a number (integer or float)
Move the turtle forward by the specified distance, in the direction
the turtle is headed.
"""
self.car.drive(RoboHatEduCar.CMD_DRV_FORWARD, distance)
def back(self, distance):
"""Move the turtle backward by distance.
Aliases: back | backward | bk
Argument:
distance -- a number
Move the turtle backward by distance ,opposite to the direction the
turtle is headed. Do not change the turtle's heading.
"""
self.car.drive(RoboHatEduCar.CMD_DRV_BACKWARD, distance)
def left(self, angle):
"""Turn turtle left by angle units.
Aliases: left | lt
Argument:
angle -- a number (integer or float)
Turn turtle left by angle degrees.
"""
self.car.turn(RoboHatEduCar.CMD_TURN_LEFT, angle=angle)
def right(self, angle):
"""Turn turtle right by angle units.
Aliases: right | rt
Argument:
angle -- a number (integer or float)
Turn turtle right by angle degrees.
"""
self.car.turn(RoboHatEduCar.CMD_TURN_RIGHT, angle=angle)
# method aliases
fd = forward
bk = back
backward = back
rt = right
lt = left