class Main(): ub = UltraBorg3.UltraBorg() tb = ThunderBorg3.ThunderBorg() MODE_STRAIGHT_LINE_SPEED = 0 MODE_OVER_THE_RAINBOW = 1 MODE_MAZE_SOLVE = 2 MODE_DERANGED_GOLF = 3 MODE_DUCK_SHOOT = 4 MODE_OBSTACLE_COURSE = 5 MODE_PI_NOON = 6 MODE_TEST = 99 mode = MODE_OBSTACLE_COURSE def __init__(self): self.ub.Init() self.tb.Init() self.t1 = datetime.now() self.tickSpeed = 0.05 self.us = Ultrasonics(self.ub) self.motors = Motors(self.tb, self.ub, self.tickSpeed) self.steering = Steering(self.tb, self.ub, self.tickSpeed) self.teleLogger = Telemetry("telemetry", "csv").get() self.ptc = PanTiltController(self.ub, 270, 135) self.joystick_controller = JoystickController(self.tb, self.ub) battMin, battMax = self.tb.GetBatteryMonitoringLimits() battCurrent = self.tb.GetBatteryReading() print('Battery monitoring settings:') print(' Minimum (red) %02.2f V' % (battMin)) print(' Half-way (yellow) %02.2f V' % ((battMin + battMax) / 2)) print(' Maximum (green) %02.2f V' % (battMax)) print() print(' Current voltage %02.2f V' % (battCurrent)) print(' Mode %s' % (self.mode)) print() def log(self): if (self.motors.speed != 0): self.teleLogger.info( '%(left)f, %(front)f, %(right)f, %(back)f, %(distanceMoved)f, %(forwardSpeed)f, %(direction)s, %(degree)f, %(ratio)f', { "left": self.us.left, "front": self.us.front, "right": self.us.right, "back": self.us.back, "distanceMoved": self.motors.distanceMoved, "forwardSpeed": self.motors.forwardSpeed, "direction": self.steering.going, "degree": self.steering.steeringPosition, "ratio": self.steering.sideRatio }) def run(self): try: if (self.mode == self.MODE_STRAIGHT_LINE_SPEED): print("Straight line speed") self.modeStraightLineSpeed() elif (self.mode == self.MODE_OVER_THE_RAINBOW): print("Rainbow") self.modeOverTheRainbow() elif (self.mode == self.MODE_DERANGED_GOLF or self.mode == self.MODE_PI_NOON or self.mode == self.MODE_OBSTACLE_COURSE): print("Joystick control: " + str(self.mode)) self.joystick_controller.run() else: # Wait for Buttons print('Pan -62 = ' + str(self.ptc.pan(-62))) time.sleep(1) print('Pan 0 = ' + str(self.ptc.pan(0))) time.sleep(1) # # print('Pan 62 = ' + str(self.ptc.pan(62))) # time.sleep(1) # # print('Pan absolute 1.0 = ' + str(self.ptc.pan_absolute(1.0))) # time.sleep(1) # # print('Pan absolute -1.0 = ' + str(self.ptc.pan_absolute(-1.0))) # time.sleep(1) # # print('Pan absolute 0.0 = ' + str(self.ptc.pan_absolute(0.0))) # time.sleep(1) # # print('Tilt -30 = ' + str(self.ptc.tilt(-30))) # time.sleep(1) # print('Tilt 0 = ' + str(self.ptc.tilt(0))) # time.sleep(1) # # print('Tilt 30 = ' + str(self.ptc.tilt(30))) # time.sleep(1) # # print('Tilt absolute 0.6 = ' + str(self.ptc.tilt_absolute(0.6))) # time.sleep(1) # # print('Tilt absolute -0.6 = ' + # str(self.ptc.tilt_absolute(-0.6))) # time.sleep(1) # # print('Tilt absolute 0.0 = ' + str(self.ptc.tilt_absolute(0.0))) # time.sleep(1) except KeyboardInterrupt: # User has pressed CTRL+C self.ub.SetServoPosition2(0) t2 = datetime.now() delta = t2 - self.t1 print("Run complete in : " + str(delta.total_seconds())) print('Done') if (self.motors): self.motors.shutdown() except Exception as e: tb = traceback.format_exc() e = sys.exc_info()[0] print(tb) if (self.motors): self.motors.shutdown() def modeStraightLineSpeed(self): self.straight_line_speed = StraightLineVision(self.steering, self.motors) self.teleLogger.info( 'left, front, right, back, distanceMoved, forwardSpeed, direction, steering position, ratio' ) self.steering.reset() slVa = VisionAttributes() slVa.startTiltAngle = 0.6 slVa.startPanAngle = 0 slVa.targetMinSize = 1000 slVa.targetMaxSize = 18000 slVa.minPanAngle = -0.5 slVa.maxPanAngle = 0.5 slVa.colour = Vision.COLOUR_WHITE slVa.targetColorPattern = Vision.COLOUR_WHITE slVa.topSpeed = 0.6 slVa.topSpinSpeed = 1.0 self.ptc.tilt(0.5) slsPtc = PanTiltController(self.ub, 270, 135) slsPtc.initPanServo(5000, 1000) self.straight_line_speed.initialise(slVa, slsPtc) self.motors.stop() prev_block_position = None t1 = datetime.now() self.straight_line_speed.track(self.straight_line_speed.COLOUR_WHITE) t2 = datetime.now() delta = t2 - self.t1 print("Run complete in: " + str(delta.total_seconds())) def modeOverTheRainbow(self): self.vision = Vision(self.steering, self.motors) slVa = VisionAttributes() slVa.startTiltAngle = 0.12 slVa.startPanAngle = -1.00 slVa.targetMinSize = 20 slVa.targetMaxSize = 2200 slVa.minPanAngle = -1.0 slVa.maxPanAngle = 1.0 slVa.targetColorPattern = Vision.COLOUR_WHITE slVa.topSpeed = 1.0 slVa.topSpinSpeed = 1.0 self.motors.move(-1, -1, 0.3) time.sleep(0.8) self.motors.stop() rainbowPtc = PanTiltController(self.ub, 270, 135) rainbowPtc.initPanServo(5000, 1000) self.vision.initialise(slVa, rainbowPtc) time.sleep(0.5) self.vision.scan() print("Scan Complete") index = 0 prev_position = 0 for ball_position in self.vision.ball_positions: ball_position = self.vision.ball_positions[0] print("Size: " + str(ball_position.size) + ', x :' + str(ball_position.x) + ', y :' + str(ball_position.y) + ', pan-position :' + str(ball_position.pan_position) + ', angle : ' + str(ball_position.pan_position * 135) + ', Colour:' + str(ball_position.colour)) if (index > 0): prev_position = self.vision.ball_positions[index - 1].pan_position index = index + 1 self.vision.goto_ball_position(ball_position, prev_position)