()
| 78 | sys.exit() # Exit the python program |
| 79 | |
| 80 | def ReadyForBalance(): |
| 81 | # float the motors |
| 82 | GPG.set_motor_power(GPG.MOTOR_LEFT + GPG.MOTOR_RIGHT, GPG.MOTOR_FLOAT) |
| 83 | |
| 84 | # create/access global variables |
| 85 | global gyroAngle |
| 86 | global mrcSum |
| 87 | global motorPos |
| 88 | global mrcDelta |
| 89 | global mrcDeltaP1 |
| 90 | global mrcDeltaP2 |
| 91 | global mrcDeltaP3 |
| 92 | global motorDiffTarget |
| 93 | global TimeOffset |
| 94 | global tInterval |
| 95 | global LastTime |
| 96 | |
| 97 | # wait for the 'OK' button to be pressed |
| 98 | print("Stand robot up and then press 'OK' on the remote.") |
| 99 | while GPG.get_grove_value(PORT_SENSOR_IR) != 3: |
| 100 | time.sleep(0.1) |
| 101 | |
| 102 | # reset the encoders |
| 103 | GPG.offset_motor_encoder(GPG.MOTOR_LEFT, GPG.get_motor_encoder(GPG.MOTOR_LEFT)) |
| 104 | GPG.offset_motor_encoder(GPG.MOTOR_RIGHT, GPG.get_motor_encoder(GPG.MOTOR_RIGHT)) |
| 105 | |
| 106 | # set variables to 0 |
| 107 | gyroAngle = 0 |
| 108 | mrcSum = 0 |
| 109 | motorPos = 0 |
| 110 | mrcDelta = 0 |
| 111 | mrcDeltaP1 = 0 |
| 112 | mrcDeltaP2 = 0 |
| 113 | mrcDeltaP3 = 0 |
| 114 | motorDiffTarget = 0 |
| 115 | TimeOffset = 0 |
| 116 | tInterval = LOOP_TIME |
| 117 | LastTime = time.time() - LOOP_TIME |
| 118 | |
| 119 | print("Balancing, so let go of the robot.") |
| 120 | print("Use Up, Down, Left, and Right on the remote to drive the robot.") |
| 121 | |
| 122 | try: |
| 123 | print("GoPiGo3 BalanceBot.") |
no test coverage detected