| 1 | import runloop
|
| 2 | import motor_pair
|
| 3 | import motor
|
| 4 | from hub import port, motion_sensor
|
| 5 |
|
| 6 | LEFT=-100
|
| 7 | RIGHT=100
|
| 8 |
|
| 9 | HOOK_RANGE=223
|
| 10 |
|
| 11 | motor_pair.pair(motor_pair.PAIR_1, port.A, port.E)
|
| 12 | top_motor=port.F
|
| 13 | bottom_motor=port.C
|
| 14 | wheel_circumference_inches=6.889764
|
| 15 | motion_sensor.set_yaw_face(motion_sensor.TOP)
|
| 16 |
|
| 17 | active_routine=0
|
| 18 |
|
| 19 | async def turn(degrees, direction, velocity=200, acceleration=1000):
|
| 20 | await turn_rotations(degrees, direction, velocity, acceleration)
|
| 21 |
|
| 22 | async def turn_rotations(degrees, direction, velocity=200, acceleration=400):
|
| 23 | await motor_pair.move_for_degrees(motor_pair.PAIR_1,
|
| 24 | int(degrees * 1.8),
|
| 25 | direction,
|
| 26 | velocity=velocity,
|
| 27 | acceleration=acceleration,
|
| 28 | deceleration=acceleration)
|
| 29 | return
|
| 30 |
|
| 31 | async def drive(distance_in_inches, steering=0, velocity=700,
|
| 32 | acceleration=500, deceleration=500):
|
| 33 | """
|
| 34 | distance_in_inches: The value to drive in inches
|
| 35 | steering: between -100 for left and 100 for right. default is 0 for straight
|
| 36 | velocity: between -1050 and 1050. default is 500
|
| 37 | acceleration: between 0 and 10000. default is 500
|
| 38 | deceleration: between 0 and 10000. default is 500
|
| 39 | """
|
| 40 | degrees = int((distance_in_inches/wheel_circumference_inches)*360)
|
| 41 |
|
| 42 | await motor_pair.move_for_degrees(motor_pair.PAIR_1, degrees, steering,
|
| 43 | velocity=velocity, acceleration=acceleration,
|
| 44 | deceleration=deceleration,
|
| 45 | stop=motor.SMART_BRAKE)
|
| 46 |
|
| 47 | def spinWheels(velocity=1000):
|
| 48 | motor_pair.move(motor_pair.PAIR_1, 0, velocity=velocity)
|
| 49 |
|
| 50 | async def lowerForks(degrees=-3360):
|
| 51 | await motor.run_for_degrees(bottom_motor, degrees, 1000)
|
| 52 |
|
| 53 | async def liftForks(degrees=3360):
|
| 54 | await motor.run_for_degrees(bottom_motor, degrees, 1000)
|
| 55 |
|
| 56 | async def lifthook(degrees=HOOK_RANGE, velocity=500):
|
| 57 | await motor.run_for_degrees(bottom_motor, -degrees, velocity)
|
| 58 |
|
| 59 | async def drophook(degrees=HOOK_RANGE, velocity=1000, acceleration=1000):
|
| 60 | await motor.run_for_degrees(bottom_motor, degrees,
|
| 61 | velocity, acceleration=acceleration)
|
| 62 |
|
| 63 | async def motorsToZero():
|
| 64 | runloop.run(
|
| 65 | motor.run_to_absolute_position(top_motor, 0, 500),
|
| 66 | motor.run_to_absolute_position(bottom_motor, 0, 500)
|
| 67 | )
|
| 68 |
|
| 69 | async def liftArm(degrees=180):
|
| 70 | await motor.run_for_degrees(bottom_motor, degrees, 500)
|
| 71 |
|
| 72 | async def dropArm(degrees=180, velocity=1110):
|
| 73 | await motor.run_for_degrees(bottom_motor, -degrees, velocity, acceleration=10000)
|
| 74 |
|
| 75 | async def easterIsland():
|
| 76 | await liftArm()
|
| 77 | await drive (17)
|
| 78 | for i in range(3):
|
| 79 | await dropArm()
|
| 80 | await liftArm()
|
| 81 | await runloop.sleep_ms(500)
|
| 82 | await drive(-17)
|
| 83 |
|
| 84 | async def wackTheTable():
|
| 85 | await liftArm()
|
| 86 | await drive(-1)
|
| 87 | await drive(14)
|
| 88 | await turn(17, LEFT)
|
| 89 | await drive(24.5,-4,500)
|
| 90 | await turn(LEFT,60)
|
| 91 | await drive(9.25)
|
| 92 | await turn(92,LEFT)
|
| 93 | await drive(-2)
|
| 94 |
|
| 95 | await dropArm(degrees=90,velocity=500)
|
| 96 | await liftArm(90)
|
| 97 | await turn(75,LEFT)
|
| 98 | await drive(12.5)
|
| 99 | await turn(50,RIGHT)
|
| 100 | await drive(32)
|
| 101 |
|
| 102 | async def theOtherSide():
|
| 103 | await drive(-1)
|
| 104 | await drive(11.2)
|
| 105 | await turn(90, LEFT)
|
| 106 | await drive(21.3)
|
| 107 | await lifthook()
|
| 108 | await turn(90, RIGHT)
|
| 109 | await drive(-0.3)
|
| 110 | await drophook()
|
| 111 | await drive(-2.5)
|
| 112 | await drive(0.2)
|
| 113 | await turn(110, RIGHT)
|
| 114 | await drive(-35, 7)
|
| 115 | await drive(-20,-3)
|
| 116 |
|
| 117 | async def liftTable():
|
| 118 | await drive(17)
|
| 119 | await turn_rotations(45, LEFT)
|
| 120 | await drive(12, -10, 700)
|
| 121 |
|
| 122 | async def brushBrushBrush():
|
| 123 | await liftForks(765)
|
| 124 | await drive(-1.67)
|
| 125 | await drive(24)
|
| 126 | await turn(90,LEFT)
|
| 127 | await turn(45,LEFT)
|
| 128 | await turn(90,RIGHT)
|
| 129 | await liftForks(900)
|
| 130 | await drive(-2.67)
|
| 131 | await turn(150,LEFT)
|
| 132 | await drive(14)
|
| 133 |
|
| 134 | async def mineIronOre():
|
| 135 | await drive(-1.67)
|
| 136 | await drive(30)
|
| 137 | await turn(90,RIGHT)
|
| 138 | await drive(14)
|
| 139 | await turn(90,LEFT)
|
| 140 | await lifthook(160)
|
| 141 | await runloop.sleep_ms(3000)
|
| 142 | await drophook(130)
|
| 143 | await turn(90,LEFT)
|
| 144 | await drive(11)
|
| 145 | await turn(75,LEFT)
|
| 146 | await drive(27.5)
|
| 147 |
|
| 148 | async def raiseTheSails():
|
| 149 | await lifthook()
|
| 150 | await drive(21.2,2)
|
| 151 | await drophook()
|
| 152 | await drive(-1.5)
|
| 153 | await lifthook()
|
| 154 | await drive(-18.5)
|
| 155 |
|
| 156 | async def loopHole():
|
| 157 | await drive(-1)
|
| 158 | await drive(28)
|
| 159 | await turn(40, LEFT)
|
| 160 | await liftForks(500)
|
| 161 | await drive(5)
|
| 162 | await liftForks(1500)
|
| 163 | await drive(-5)
|
| 164 | await turn(40,RIGHT)
|
| 165 | await drive(-29)
|
| 166 |
|
| 167 | async def main():
|
| 168 |
|
| 169 |
|
| 170 |
|
| 171 | await easterIsland()
|
| 172 |
|
| 173 |
|
| 174 |
|
| 175 |
|
| 176 | await wackTheTable()
|
| 177 |
|
| 178 |
|
| 179 | await raiseTheSails()
|
| 180 |
|
| 181 |
|
| 182 | await theOtherSide()
|
| 183 |
|
| 184 |
|
| 185 | await brushBrushBrush()
|
| 186 |
|
| 187 |
|
| 188 | await mineIronOre()
|
| 189 |
|
| 190 |
|
| 191 | await loopHole()
|
| 192 |
|
| 193 | raise SystemExit
|
| 194 |
|
| 195 | runloop.run(main()) |