-
Notifications
You must be signed in to change notification settings - Fork 1
Expand file tree
/
Copy pathtests.py
More file actions
74 lines (56 loc) · 1.62 KB
/
tests.py
File metadata and controls
74 lines (56 loc) · 1.62 KB
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
from pybricks.tools import print, wait
from pybricks import ev3brick as brick
from pybricks.ev3devices import GyroSensor
from pybricks.parameters import Port
from robot import Robot
def printMsg(msg):
brick.display.text(msg)
print(msg)
def test_drive():
r = Robot(debug=True)
inches = 10.
r.drive_straight_inches(200., inches)
# r.spin_right_to_angle(200., 90)
# r.spin_left_to_angle(400, -90)
def display_gyro_values():
print("HW reset, then btn press:")
while not any(brick.buttons()):
wait(50)
robot = Robot()
wait(500)
robot.reset_gyro_angle()
wait(500)
while True:
angle = robot.get_gyro_angle()
print(angle)
wait(50)
def test_gyro_drift(port=Port.S4):
gyro = GyroSensor(port)
printMsg("Checking for drift.")
numTests = 5
drifting = False
for i in range(numTests):
angle = gyro.angle()
printMsg("Test %i/%i = %d" % (i+1, numTests, angle))
if angle != 0:
drifting = True
break # no need to keep testing
wait(1000)
return drifting
def prepare_gyro(port=Port.S4):
"""
Run this before you use the gyro sensor to try and
detect issues, and fix them.
"""
drifting = True
while drifting:
drifting = test_gyro_drift(port=port)
if drifting:
printMsg("Detected Drift!")
printMsg("Perform HW Cal,")
printMsg("Then press any btn.")
while not any(brick.buttons()):
wait(50)
else:
printMsg("No Drift Detected.")
printMsg("Gyro ready for Use!")