(trigger, put_on_hold, simultaneous_launcher, sensor_queue)
| 183 | |
| 184 | |
| 185 | def robotController(trigger, put_on_hold, simultaneous_launcher, sensor_queue): |
| 186 | # try to connect to the GoPiGo3 |
| 187 | try: |
| 188 | gopigo3_robot = EasyGoPiGo3() |
| 189 | except IOError: |
| 190 | print("GoPiGo3 robot not detected") |
| 191 | simultaneous_launcher.abort() |
| 192 | except gopigo3.FirmwareVersionError: |
| 193 | print("GoPiGo3 board needs to be updated") |
| 194 | simultaneous_launcher.abort() |
| 195 | except Exception: |
| 196 | print("Unknown error occurred while instantiating GoPiGo3") |
| 197 | simultaneous_launcher.abort() |
| 198 | |
| 199 | # set a lower speed of the GoPiGo3 |
| 200 | gopigo3_robot.set_speed(300) |
| 201 | gopigo3_robot.stop() # in case the GoPiGo3 is moving, stop it |
| 202 | previous = 0 # see the following instructions |
| 203 | how_much_of_distance = 0.30 # measured in percentage |
| 204 | |
| 205 | # if the robot is unplugged from the battery pack |
| 206 | # or if the batteries are low |
| 207 | # then abort everything |
| 208 | if gopigo3_robot.volt() <= MINIMUM_VOLTAGE: |
| 209 | print("Voltage too low") |
| 210 | simultaneous_launcher.abort() |
| 211 | |
| 212 | # check if an error has occurred during all the above processes |
| 213 | try: |
| 214 | simultaneous_launcher.wait() |
| 215 | except threading.BrokenBarrierError: |
| 216 | print("[robotController] thread couldn't be launched") |
| 217 | |
| 218 | if not simultaneous_launcher.broken: |
| 219 | print("[robotController] thread fully active") |
| 220 | |
| 221 | # if everything is fine |
| 222 | # start polling messages from [obstacleFinder] function (which is thread-launched) |
| 223 | while not trigger.is_set() and not simultaneous_launcher.broken: |
| 224 | try: |
| 225 | (distance_to_walk, rotation) = sensor_queue.get_nowait() |
| 226 | sensor_queue.task_done() |
| 227 | except queue.Empty: |
| 228 | sleep(0.001) |
| 229 | continue |
| 230 | |
| 231 | print("Rotating at {} degrees and driving for {} cm".format(rotation, distance_to_walk * how_much_of_distance)) |
| 232 | |
| 233 | put_on_hold.set() |
| 234 | gopigo3_robot.turn_degrees(rotation, blocking = True) |
| 235 | gopigo3_robot.drive_cm(distance_to_walk * how_much_of_distance, blocking = False) |
| 236 | |
| 237 | # give some time for the robot to start moving |
| 238 | # before taking measurements of its speed |
| 239 | sleep(0.3) |
| 240 | |
| 241 | # while the robot is moving and CTRL-C hasn't been pressed |
| 242 | while gopigo3_robot.get_motor_status(gopigo3_robot.MOTOR_LEFT)[3] != 0 and \ |
nothing calls this directly
no test coverage detected