class Camera: def __init__(self): config = configparser.ConfigParser() config.read("./config.ini") HIntfNum = config.getint("camera", "HIntfNum") HInitPosition = config.getint("camera", "HInitPosition") HMinPosition = config.getint("camera", "HMinPosition") HMaxPosition = config.getint("camera", "HMaxPosition") HSpeed = config.getint("camera", "HSpeed") # Vertical direction control parameters VIntfNum = config.getint("camera", "VIntfNum") VInitPosition = config.getint("camera", "VInitPosition") VMinPosition = config.getint("camera", "VMinPosition") VMaxPosition = config.getint("camera", "VMaxPosition") VSpeed = config.getint("camera", "VSpeed") self.HCameraControl = Steering(HIntfNum, HInitPosition, HMinPosition, HMaxPosition, HSpeed) self.VCameraControl = Steering(VIntfNum, VInitPosition, VMinPosition, VMaxPosition, VSpeed) def cameraRotate(self, direction): if direction == "A": self.HCameraControl.forwardRotation() elif direction == "D": self.HCameraControl.reverseRotation() elif direction == "W": self.VCameraControl.forwardRotation() elif direction == "S": self.VCameraControl.reverseRotation() elif direction == "R": self.HCameraControl.reset() self.VCameraControl.reset() else: print( "Your input for camera direction is wrong, please input: D, A, W, S or RESET!" )
class Sense(): def __init__(self): config = configparser.ConfigParser() config.read("config.ini") trigger = config.getint("ultrasonic", "Trigger") echo = config.getint("ultrasonic", "Echo") servoNum = config.getint("steering", "servoNum") InitPosition = config.getint("steering", "InitPosition") minPosition = config.getint("steering", "MinPosition") maxPosition = config.getint("steering", "maxPosition") speed = config.getint("steering", "speed") self.IN_1 = config.getint("car", "IN1") self.IN_2 = config.getint("car", "IN2") self.IN_3 = config.getint("car", "IN3") self.IN_4 = config.getint("car", "IN4") self.Sonic = Ultrasonic(trigger, echo) self.Servo = Steering(servoNum, InitPosition, minPosition, maxPosition, speed) self.motor = Motor(self.IN_1, self.IN_2, self.IN_3, self.IN_4) #get obstacles type def detection(self): distance = self.ask_distance() #NO Obstacles or 35cm safe distance type 0 if distance >= 30: return "Fgo" #Obstacles ahead #self.Infrared.check_distance() == "Warning" else: l_d = self.ask_distance_l() r_d = self.ask_distance_r() #Obstacles ahead&&right =safe type a if ((l_d >= 30) and (r_d >= 30)): if (l_d > r_d): return "Lgo" else: return "Rgo" #Obstacles ahead&&right R>L type b elif ((l_d <= 30) and (r_d > 30)): return "Rgo" #Obstacles ahead&&right L>R type c elif ((l_d > 30) and (r_d <= 30)): return "Lgo" #Obstacles ahead&&right L&R<safe type d elif ((l_d < 30) and (r_d < 30)): return "Bgo" def ask_distance(self): self.Servo.reset() return self.Sonic.get_distance() def ask_distance_l(self): self.motor.stop() self.Servo.turnleft() time.sleep(0.1) distance = self.Sonic.get_distance() self.Servo.reset() return distance def ask_distance_r(self): self.motor.stop() self.Servo.turnright() time.sleep(0.1) distance = self.Sonic.get_distance() self.Servo.reset() return distance
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)
class Camera: def __init__(self): ''' Read config file to init camera's parameter ''' config = configparser.ConfigParser() config.read("config.ini") self.statu="" # Horiazonal direction control parameters HIntfNum = config.getint("camera", "HIntfNum") HInitPosition = config.getint("camera", "HInitPosition") HMinPosition = config.getint("camera", "HMinPosition") HMaxPosition = config.getint("camera", "HMaxPosition") HSpeed = config.getint("camera", "HSpeed") # Vertical direction control parameters VIntfNum = config.getint("camera", "VIntfNum") VInitPosition = config.getint("camera", "VInitPosition") VMinPosition = config.getint("camera", "VMinPosition") VMaxPosition = config.getint("camera", "VMaxPosition") VSpeed = config.getint("camera", "VSpeed") self.HCameraControl = Steering( HIntfNum, HInitPosition, HMinPosition, HMaxPosition, HSpeed) self.VCameraControl = Steering( VIntfNum, VInitPosition, VMinPosition, VMaxPosition, VSpeed) def cameraRotate(self, direction): ''' This method is used to contorl the camera's rotating The value of parameter direction and its meaning as follow: HR - Turn right HL - Turn left VU - Turn upward VD - Turn downword ''' if direction == "HL": self.HCameraControl.forwardRotation() config = configparser.ConfigParser() config.read("config.ini") H = self.HCameraControl.position config.set("camera", "HInitPosition", str(H)) with open("config.ini", "w+") as f: config.write(f) self.statu="LEFT" elif direction == "HR": self.HCameraControl.reverseRotation() config = configparser.ConfigParser() config.read("config.ini") H = self.HCameraControl.position config.set("camera", "HInitPosition", str(H)) with open("config.ini", "w+") as f: config.write(f) self.statu = "RIGHT" elif direction == "VU": self.VCameraControl.forwardRotation() config = configparser.ConfigParser() config.read("config.ini") V = self.VCameraControl.position config.set("camera", "VInitPosition", str(V)) with open("config.ini", "w+") as f: config.write(f) self.statu = "UP" elif direction == "VD": self.VCameraControl.reverseRotation() config = configparser.ConfigParser() config.read("config.ini") V = self.VCameraControl.position config.set("camera", "VInitPosition", str(V)) with open("config.ini", "w+") as f: config.write(f) self.statu = "DOWN" elif direction == "RESET": self.HCameraControl.reset() self.VCameraControl.reset() config = configparser.ConfigParser() config.read("config.ini") H = 80 V = 50 config.set("camera", "HInitPosition", str(H)) config.set("camera", "VInitPosition", str(V)) with open("config.ini", "w+") as f: config.write(f) self.statu = "RESET" else: print( "Your input for camera direction is wrong, please input: HR, HL, VU, VD or RESET!") def __del__(self): print(self.statu+" over and "+"camera has been deleted")