-
Notifications
You must be signed in to change notification settings - Fork 0
/
drive.py
279 lines (236 loc) · 8.23 KB
/
drive.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
"""
this module provides drive functionality for create
"""
import math
from kipr import *
from time import time, sleep
import constants as c
import utilities as u
from createserial.commands import create_dd
from createserial.encoders import Encoders
import actions as a
from sensors import read_cliffs, on_white
GYRO_OFFSET = 0
def drive(l_speed: int, r_speed: int):
"""Drives left and right motor for values between -100 to 100"""
create_dd(r_speed * -5, (int(l_speed * -5 * c.ADJUST_SPEED)))
def drive_timed(l_speed: int, r_speed: int, drive_time: int):
"""Drives left and right for time milliseconds and motor values between -100 and 100"""
drive(l_speed, r_speed)
msleep(drive_time)
u.stop()
def drive_distance_default(speed: int, distance: float):
converted = (speed * 5) / 25.4 # mm/sec to in/sec
ms = abs(int((distance / converted) * 1000))
drive_timed(speed, speed, ms)
def calibrate_gyro():
global GYRO_OFFSET
total = 0
for _ in range(100):
total += gyro_z()
msleep(10)
GYRO_OFFSET = total / 100
def calibrated_gyro_z():
return gyro_z() - GYRO_OFFSET
def drive_distance_straight(speed: int, distance: float):
end_time = time() + abs(distance * 25.4 / speed / 5)
r_speed = l_speed = speed
while time() < end_time:
msleep(10)
drive(l_speed, r_speed)
if calibrated_gyro_z() > 10:
r_speed += 3
l_speed -= 1
elif calibrated_gyro_z() < -10:
r_speed -= 3
l_speed += 1
else:
r_speed = l_speed = speed
u.stop()
def drive_distance_straight_2(speed: int, distance):
end_time = time() + abs(distance * 25.4 / speed / 5)
r_speed = l_speed = speed
tot_offset = 0
while time() < end_time:
tot_offset += calibrated_gyro_z()
drive(l_speed, r_speed)
if tot_offset > 10:
r_speed += 1
l_speed -= 1
elif tot_offset < -10:
l_speed += 1
r_speed -= 1
else:
r_speed = l_speed = speed
msleep(10)
u.stop()
def pivot(speed, angle, stationary_wheel):
arc_length = 2 * (angle * 9.25 * math.pi) / 360
encoders = Encoders()
left, right = encoders.values
r_speed = l_speed = speed * 5
inches = 0
while abs(inches) < abs(arc_length):
msleep(15)
left, right = encoders.values
if stationary_wheel == "left":
inches = (right * (math.pi * 72 / 508.8) / 24.5)
create_dd(0, r_speed)
else:
inches = (left * (math.pi * 72 / 508.8) / 24.5)
create_dd(l_speed, 0)
# print("in loop", left, right, inches, l_speed, r_speed, arc_length)
drive(0, 0)
def spin(speed, angle):
arc_length = (angle * 9 * math.pi) / 360
encoders = Encoders()
left, right = encoders.values
right = -1 * right
left = -1 * left
r_speed = l_speed = speed * 5
inches = 0
create_dd(-l_speed, r_speed)
while abs(inches) < abs(arc_length):
# msleep(15)
left, right = encoders.values
inches = abs(right) * ((math.pi * 72 / 508.8) / 24.5) # + (abs(left) * (math.pi * 72 / 508.8) / 24.5)
# print("in loop", left, right, inches, l_speed, r_speed, arc_length)
drive(0, 0)
def spin_to_black(speed):
r_speed = l_speed = speed * 5
while analog_et(0) < c.TOPHAT_THRESHOLD:
msleep(15)
create_dd(-l_speed, r_speed)
# print(analog_et(0))
drive(0, 0)
def spin_to_white(speed):
r_speed = l_speed = speed * 5
while analog_et(0) > c.TOPHAT_THRESHOLD:
msleep(15)
create_dd(-l_speed, r_speed)
# print(analog_et(0))
drive(0, 0)
def spin_to_black_2(speed):
r_speed = l_speed = speed * 5
if analog_et(0) < c.TOPHAT_THRESHOLD:
print("on white, spinning to black")
create_dd(-l_speed, r_speed) # or drive ?
while analog_et(0) < c.TOPHAT_THRESHOLD:
pass
# print(analog_et(0))
drive(0, 0)
def spin_to_white_2(speed):
r_speed = l_speed = speed * 5
if analog_et(0) > c.TOPHAT_THRESHOLD:
create_dd(-l_speed, r_speed) # or drive ?
while analog_et(0) > c.TOPHAT_THRESHOLD:
pass
# print(analog_et(0))
drive(0, 0)
def drive_until_black(speed):
drive(speed, int(speed * 0.80)) # 0.85 same for prime and clone?
rCliff, lCliff = read_cliffs()
while rCliff > 1500 and lCliff > 1000:
rCliff, lCliff = read_cliffs()
drive(0, 0)
def drive_until_black_square(speed):
drive(speed, int(speed * 0.80)) # 0.85 same for prime and clone?
l_speed = r_speed = speed
while True:
rCliff, lCliff = read_cliffs()
if rCliff < u.pc(1500, 1700):
r_speed = 0
drive(l_speed, r_speed)
# print("right on black")
if lCliff < 1500:
l_speed = 0
drive(l_speed, r_speed)
# print("left on black")
if l_speed == 0 and r_speed == 0:
break
def drive_until_white(speed):
drive(speed, int(speed * 0.80)) # 0.85 same for prime and clone?
rCliff, lCliff = read_cliffs()
while rCliff < 1500 and lCliff < 1000:
rCliff, lCliff = read_cliffs()
drive(0, 0)
def arc_to_black(speed, direction):
if direction == "r":
l_speed = int(speed / 8)
r_speed = speed
if direction == "l":
l_speed = speed
r_speed = int(speed / 8)
drive(l_speed, r_speed)
while True:
rCliff, lCliff = read_cliffs()
if rCliff < 1500:
r_speed = 0
drive(l_speed, r_speed)
if lCliff < 1500:
l_speed = 0
drive(l_speed, r_speed)
if l_speed == 0 and r_speed == 0:
break
drive(0, 0)
def drive_with_line_follow(speed, distance): # distance is in inches
encoders = Encoders()
right, left = encoders.values
right = -1 * right
left = -1 * left
r_speed = l_speed = speed
inches = 0
create_dd(r_speed * -5, l_speed * -5)
while abs(inches) < abs(distance):
sensor_value = analog12(c.TOP_HAT)
# print(sensor_value)
# if sensor_value >= 1700:
# create_dd(((r_speed - 3) * -5), l_speed * -5)
# elif 900 < sensor_value < 1700:
# create_dd(r_speed * -5, l_speed * -5)
# else:
# create_dd(r_speed * -5, (l_speed - 3) * -5)
val_difference = round((1050 - sensor_value) / 200)
# print(val_difference)
if 700 < sensor_value < 1400:
# print("go straight")
create_dd(r_speed * -5, l_speed * -5)
else:
if val_difference > 0:
create_dd(r_speed * -5, int(l_speed - val_difference) * -5)
# print("speed: ", int(l_speed - val_difference) * -5, "turn left")
else:
create_dd((int(r_speed - abs(val_difference)) * -5), l_speed * -5)
# print("speed: ", (int(r_speed - abs(val_difference)) * -5), "turn right")
right, left = encoders.values
right = -1 * right
left = -1 * left
inches = 0.5 * ((right * (math.pi * 72 / 508.8) / 24.5) + (left * (math.pi * 72 / 508.8) / 24.5))
drive(0, 0)
# encoder values to inches: n * (math.pi * 72 / 508.8) / 24.5 where n equals encoder values
def drive_straight(speed, distance: float):
p = 0.25 # was p = 0.30
i = 0.05 # was i = 0.05
encoders = Encoders()
right, left = encoders.values
right = -1 * right
left = -1 * left
old_left, old_right = left, right
r_speed = l_speed = speed
inches = 0
create_dd(r_speed * -5, int(l_speed * -5 * c.ADJUST_SPEED))
while abs(inches) < abs(distance):
# msleep(15)
right, left = encoders.values
right = -1 * right
left = -1 * left
inches = 0.5 * ((right * (math.pi * 72 / 508.8) / 24.5) + (left * (math.pi * 72 / 508.8) / 24.5))
p_error = (right - old_right) - (left - old_left) # short term
i_error = right - left # long term
# print(p_error, i_error, p * p_error, i * i_error)
r_speed -= int(p * p_error + i * i_error)
l_speed += int(p * p_error + i * i_error)
create_dd(r_speed * -5, int(l_speed * -5 * c.ADJUST_SPEED))
old_left = left
old_right = right
drive(0, 0)