code\2024-dive.py

1"""
2This is the code for the 2024 FLL Season Dive/Submerged Challenge.
3Our team uses a single program, but comments out all but one mission at a time in main and
4uploads the code to the hub in a specific slot.
5
6Slot | Function | Description
7-----|------------|---------------------------------------------------
81 | whackShark | Hit the shark, hit water sample, flip coral buds, get back home
92 | moveShark | Drop shark off, get the octopus, go home on the right side
103 | moveOcto | Drop octo off, get plankton sample, move angler fish, go to left home
114 | liftMast | Raise the mast of the ship
125 | moveCoral | Optional mission. Move a coral that is in home, out of home
13"""
14
15import runloop
16import motor_pair
17from hub import port, motion_sensor
18
19import motor
20
21TURN_BY="DEGREES"
22
23LEFT=-100
24RIGHT=100
25
26HOOK_RANGE=217
27
28motor_pair.pair(motor_pair.PAIR_1, port.A, port.E)
29top_motor=port.F
30bottom_motor=port.C
31wheel_circumference_­inches=6.889764 # 17.5cm / 2.54cm per inch
32motion_sensor.set_yaw_face(motion_sensor.TOP)
33
34async def turn(degrees, direction, velocity=200, acceleration=1000):
35 """
36 Turns the robot by a the number of degrees using the default method (gyro, rotations)
37 degrees: The value to turn in degrees
38 direction: Either -100 for left or 100 for right (set as the constants LEFT and RIGHT).
39 velocity: between -1050 and 1050. default is 200
40 acceleration: between 0 and 10000. default is 1000. Also used for deceleration
41 """
42 if TURN_BY == "DEGREES":
43 await turn_gyro(degrees if direction == RIGHT else -degrees, velocity, acceleration)
44 else:
45 await turn_rotations(degrees, direction, velocity, acceleration)
46
47async def turn_rotations(degrees, direction, velocity=200, acceleration=400):
48 """
49 Turn the robot a certain number of degrees in a certain direction, using a calculation
50 to determine the number of degrees of wheel rotation that would rotate the robot's
51 facing direction that many degrees. Wheel size and wheelbase would change this value.
52 1.8 was the value that the team found by trial and error to be the correct value for the
53 robot they were using.
54
55 degrees: The value to turn in degrees
56 direction: Either -100 for left or 100 for right (set as the constants LEFT and RIGHT).
57 velocity: between -1050 and 1050. default is 200
58 acceleration: between 0 and 10000. default is 1000. Also used for deceleration
59 """
60 # convert the degrees of desired turn to the number of degrees the motors need to turn
61 await motor_pair.move_for_degrees(
62 motor_pair.PAIR_1,
63 int(degrees * 1.8),
64 direction,
65 velocity=velocity,
66 acceleration=acceleration,
67 deceleration=acceleration
68 )
69 return
70
71async def turn_gyro(angle, velocity=100, acceleration=1000):
72 """
73 Turn the robot a certain number of degrees in a certain direction, using yaw value
74 from the motion sensor. This should be accurate and not require any adjustments
75 for wheel size or surface, but in practice we have noticed anomalies, and in fact we
76 occasionally needed to restart the hub to get it to work at all.
77 angle: The number of degrees to turn. Positive is right, negative is left.
78 velocity: between -1050 and 1050. default is 100
79 acceleration: between 0 and 10000. default is 1000
80 """
81
82 # try to wait for the robot to be stable with the current direction set to 0
83 motion_sensor.reset_yaw(0)
84 await runloop.until(motion_sensor.stable)
85 await runloop.sleep_ms(200)
86 await runloop.until(motion_sensor.stable)
87
88 # set steering for either a left or right turn based on positive or negative angle
89 steering = 100 if angle >= 0 else -100
90
91 # start turning
92 motor_pair.move(
93 motor_pair.PAIR_1,
94 steering,
95 velocity=velocity,
96 acceleration=acceleration
97 )
98
99 def done():
100 # get yaw in decidegrees (tenths of a degree), flip sign, and convert to degrees
101 yaw_deg = motion_sensor.tilt_angles()[0] * -0.1
102 # return false until the robot has turned the desired angle
103 return abs(yaw_deg) >= abs(angle)
104
105 # sleep until the robot has turned the desired angle and then stop the drive motors
106 await runloop.until(done)
107 motor_pair.stop(motor_pair.PAIR_1)
108
109async def drive(distance_in_inches, steering=0, velocity=700, acceleration=500):
110 """
111 distance_in_inches: The value to drive in inches
112 steering: between -100 for left and 100 for right. default is 0 for straight
113 velocity: between -1050 and 1050. default is 500
114 acceleration: between 0 and 10000. default is 500
115 deceleration: between 0 and 10000. default is 500
116 """
117 degrees = int((distance_in_inches/wheel_circumference_­inches)*360)
118
119 await motor_pair.move_for_degrees(
120 motor_pair.PAIR_1,
121 degrees,
122 steering,
123 velocity=velocity,
124 acceleration=acceleration,
125 deceleration=acceleration,
126 stop=motor.SMART_BRAKE
127 )
128
129async def liftArm(degrees=180):
130 """
131 Lift the arm (with the extra bricks for weight) of the robot.
132 degrees: The number of degrees to lift the arm. Default is 180, but can be changed.
133 """
134 await motor.run_for_degrees(bottom_motor, degrees, 500)
135
136async def dropArm(degrees=180):
137 """
138 Drop the arm (with the extra bricks for weight) of the robot.
139 degrees: The number of degrees to drop the arm. Default is 180, but can be changed.
140 """
141 await motor.run_for_degrees(bottom_motor, -degrees, 1110, acceleration=10000)
142
143async def lifthook(degrees=HOOK_RANGE, velocity=500):
144 """
145 Lift the hook (attachment with single curved "hook" pointing down) of the robot.
146 degrees: The number of degrees to lift the hook. Default is 217, but can be changed.
147 velocity: between -1050 and 1050. default is 500. How fast to lift the hook.
148 """
149 await motor.run_for_degrees(bottom_motor, -degrees, velocity)
150
151async def drophook(degrees=HOOK_RANGE, velocity=1000, acceleration=1000):
152 """
153 Drop the hook (attachment with single curved "hook" pointing down) of the robot.
154 degrees: The number of degrees to drop the hook. Default is 217, but can be changed.
155 velocity: between -1050 and 1050. default is 500. How fast to drop the hook.
156 """
157 await motor.run_for_degrees(bottom_motor, degrees, velocity, acceleration=acceleration)
158
159async def whackShark():
160 """
161 Whack the shark, back up into the water sample, flip the coral buds, then get back home
162 Line up to cover the d in education with the right side of the robot
163 """
164 straight_distance = 30
165 last_distance = 2.6
166
167 # move to the shark
168 await drive(straight_distance)
169 await turn(40,LEFT)
170 await liftArm()
171 await drive(last_distance)
172
173 # hit the shark
174 await dropArm()
175 await runloop.sleep_ms(500)
176 await liftArm()
177
178 # back up
179 await drive(-3)
180 await dropArm()
181 await drive(-9.5, 17)
182 await drive(5.2)
183 await drive(-2.2)
184 await turn_rotations(45, LEFT)
185 await drive(25, -10, 700)
186
187async def moveShark():
188 """
189 Drop the shark off in the habitat, then get the octopus and go to the right side home
190 Line up backward, facing the other side of the board. left edge aligned with the boxes.
191 Roll robot backwards to preset wheels. Attach pusher attachment and put the octo slide
192 on the top of the robot
193 """
194
195 await drive(-31,4)
196 await drive(10,5)
197 await drive(-50,-3)
198 await drive(17,5)
199 await drive(-17,0)
200
201async def moveOcto():
202 """
203 Move the octopus to the drop off, then get the plankton sample, head to the angler fish,
204 and get back home on the left side of the board
205 Line up left side to 1st major line from the left
206 """
207
208 # Get to the drop off
209 await drive(-19.5)
210 await drive(-17,20)
211 await drive(-9,0)
212
213 # back up to the plankton sample and grab it
214 await drive(20,1)
215 await drophook()
216
217 # pull away to remove the plankton sample
218 await drive(-19,-5)
219 await drive(-10,4)
220
221 # lift the hook and turn to the left
222 await lifthook()
223 await drive(-3.5,10)
224 await turn(73,LEFT)
225
226 # drive to the angler fish and turn into the lever
227 await drive(-5)
228 await turn_rotations(100,RIGHT)
229
230 # get back home
231 await drive(-25,11)
232 await drive(-28)
233
234async def liftMast():
235 """
236 Raise the mast of the ship. Flips it all the way up, then pulls it back down
237 Line up left side to 2nd major line from the left, facing "north"
238 """
239 await drophook()
240 await drive(20.5)
241 await turn_rotations(90,RIGHT)
242 await drive(7.8)
243 await lifthook(217,102)
244 await drive(4.1)
245 await drophook(70)
246
247 # drive while dropping the hook at the same time
248 runloop.run(drive(-3), drophook(70, velocity=100))
249
250 # go back home
251 await lifthook(140)
252 await drive(-7.8)
253 await turn_rotations(90,RIGHT)
254 await drive(12)
255
256async def moveCoral():
257 """
258 Optional mission. Move a coral that is in home, out of home
259 Line up close to the edge of home
260 """
261
262 await drive(-16,0,500)
263 await drive(18,0)
264
265async def main():
266 # loaded as #1 - line up to the back, right the right side over the d in education
267 # uses the the flipper attachment
268 await whackShark()
269
270 # loaded as #2 - line up in reverse facing the other side of the board.
271 # left side side aligned with the boxes (over the logo).
272 # Roll robot backwards to preset wheels. Attach pusher attachment
273 await moveShark()
274
275 # loaded as #3 - line up left side to 1st major line from the left
276 await moveOcto()
277
278 # loaded as #4 - line up left side to 2nd major line from the left
279 await liftMast()
280
281 # loaded as #5 - line up close to the edge
282 await moveCoral()
283
284 raise SystemExit
285
286runloop.run(main())