-
Notifications
You must be signed in to change notification settings - Fork 0
/
calibrate.py
159 lines (142 loc) · 4.93 KB
/
calibrate.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
#!/usr/local/bin/python3.10 -u
from kipr import b_button, freeze, c_button, a_button, get_motor_position_counter, clear_motor_position_counter, \
push_button
from common import ROBOT
from constants.ports import LEFT_MOTOR, RIGHT_MOTOR
from drive import drive, straight_timed_fast, straight_distance_fast, straight_distance_slow, stop_motors
from utilities import wait_for_button
# def choose_to_calibrate_adam():
# while not push_button():
# print("Press 'B' to calibrate drive, push button to skip")
# while True:
# if b_button():
# while b_button():
# pass
# run_calibration()
# break
# elif push_button():
# break
# break
# while push_button():
# pass
def choose_to_calibrate():
print("Press 'B' to calibrate drive, push button to skip")
while not push_button():
if b_button():
while b_button():
pass
run_calibration()
while push_button():
pass
# Press the green button in the gutter to run the script.
def run_calibration():
calibrate_drive_offset(85, "fast", 5000)
wait_for_button()
calibrate_drive_offset(45, "slow", 8000)
wait_for_button()
calibrate_distance_from_ticks(10000)
wait_for_button()
print("fast for 36 inches")
straight_distance_fast(36)
print("slow for 36 inches")
straight_distance_slow(36)
wait_for_button()
print("fast backwards for 36 inches")
straight_distance_fast(-36)
print("slow backwards for 36 inches")
straight_distance_slow(-36)
print("done calibrating :)")
def calibrate_drive_offset(base_speed, name: str, duration=7500):
print("Calibrate drive for", name)
print("A go more right, C go more left, B when done")
offset = ROBOT.load(name) or 0
print("Starting with offset", offset)
starting_left = base_speed
starting_right = base_speed
while not b_button():
print(starting_left, starting_right + offset)
drive(starting_left, starting_right + offset, duration)
stop_motors()
while True:
if a_button():
print("Went too far left - offset decreased")
offset -= 1
break
elif c_button():
print("Went too far right - offset increased")
offset += 1
break
elif b_button():
break
while b_button():
pass
ROBOT.store(name, offset)
print("Drive adjusted! New offset:", offset)
# def calibrate_distance_from_inches(inches):
# print("Calibrate drive distance for", inches, "inches")
# conversion = 1000
# while not b_button():
# clear_motor_position_counter(LEFT_MOTOR)
# while get_motor_position_counter(LEFT_MOTOR) < conversion * inches:
# straight_timed_fast(0)
# stop_motors()
# while True:
# if c_button():
# while c_button():
# pass
# print("went too far right - offset decreased")
# conversion -= 100
# elif a_button():
# while a_button():
# pass
# print("went too far left - offset increased")
# conversion += 100
# elif b_button():
# while b_button():
# pass
# break
# ROBOT.store("inches_to_ticks", conversion)
# print("Drive adjusted! New conversion value:", conversion)
def get_a_number(default=0):
print("A to increase value, C to decrease value, B when done")
print("Starting value:", default)
number = default
while True:
if c_button():
while c_button():
pass
number -= 1
print("Current value:", number)
elif a_button():
while a_button():
pass
number += 1
print("Current value:", number)
elif b_button():
while b_button():
pass
print("Current value:", number)
break
while b_button():
pass
return number
def calibrate_distance_from_ticks(ticks):
print("Calibrate drive distance for", ticks, "ticks")
clear_motor_position_counter(LEFT_MOTOR)
while get_motor_position_counter(LEFT_MOTOR) < ticks:
straight_timed_fast(0, stop=False)
stop_motors()
print("Enter the number of inches that the robot drove")
ones = get_a_number(50)
print("Enter the additional tenths of inches that the robot drove")
tenths = get_a_number()
inches = ones + 0.1 * tenths
conversion = ticks / inches
ROBOT.store("inches_to_ticks", conversion)
print("Drive adjusted! New conversion value:", conversion)
if __name__ == '__main__':
run_calibration()
straight_distance_fast(3*12)
wait_for_button()
straight_distance_slow(3*12)
print("done!")