def __init__(self): self.logging = False self.setDefaults() self.interface = brickpi.Interface() self.interface.initialize() self.events = Events() self.motorL = Motor(self.interface, self.events, 0) self.motorR = Motor(self.interface, self.events, 1) self.initMotorParams(self.motorL.motorParams) self.initMotorParams(self.motorR.motorParams) self.initConfig() self.touchSensorL = PushSensor('left', self.interface, 0, self.events, brickpi.SensorType.SENSOR_TOUCH) self.touchSensorR = PushSensor('right', self.interface, 1, self.events, brickpi.SensorType.SENSOR_TOUCH) Bumper(self.events) self.ultraSonic = UltraSonicSensor(self.interface, 2, self.events, brickpi.SensorType.SENSOR_ULTRASONIC) self.setPID(self.pidk_p, self.pidk_i, self.pidk_d) self.events.add(EventType.SENSOR_TOUCH, self.sensorAction)
class Robot: ###################### ### Initialisation ### ###################### """ Sets up the standard motor parameters (excluding PID values) """ def initMotorParams(self, motorParams): motorParams.maxRotationAcceleration = 10 motorParams.maxRotationSpeed = 20 motorParams.feedForwardGain = 255/20 motorParams.minPWM = 18 motorParams.pidParameters.minOutput = -255 motorParams.pidParameters.maxOutput = 255 """ Reads robot configuration from the config file """ def initConfig(self): config = RawConfigParser() config.optionxform = str config.read('robot.cfg') for (item, value) in config.items('Robot'): setattr(self, item, float(value)) print("Robot config loaded") """ Sets default configuration values - call this before `initConfig` """ def setDefaults(self): # The time between successive `check` calls self.deltaTime = 0.1 # Overall power coefficient to the motors self.powerL = 1 self.powerR = 1 # Overall coefficient for rotation self.rotatePower = 0.95 # Number of radians to one metre of movement self.movementCoeff = 36.363636 # The centre-to-wheel distance of the robot self.botRadius = 0.08 # Motor PID values self.pidk_p = 600 self.pidk_i = 100 self.pidk_d = 20 def __init__(self): self.logging = False self.setDefaults() self.interface = brickpi.Interface() self.interface.initialize() self.events = Events() self.motorL = Motor(self.interface, self.events, 0) self.motorR = Motor(self.interface, self.events, 1) self.initMotorParams(self.motorL.motorParams) self.initMotorParams(self.motorR.motorParams) self.initConfig() self.touchSensorL = PushSensor('left', self.interface, 0, self.events, brickpi.SensorType.SENSOR_TOUCH) self.touchSensorR = PushSensor('right', self.interface, 1, self.events, brickpi.SensorType.SENSOR_TOUCH) Bumper(self.events) self.ultraSonic = UltraSonicSensor(self.interface, 2, self.events, brickpi.SensorType.SENSOR_ULTRASONIC) self.setPID(self.pidk_p, self.pidk_i, self.pidk_d) self.events.add(EventType.SENSOR_TOUCH, self.sensorAction) ############### ### Logging ### ############### """ Set whether to enable logging """ def setLogging(self, log): self.logging = log """ Starts logging on the current interface `optargs` is a list of identifying values for the log file name """ def startLogging(self, optargs): if self.logging: optargs = [optarg for optarg in optargs if optarg is not None] optargs_str = '_'.join(str(optarg) for optarg in optargs) self.logName = './logs/' + \ str(int(time.time())) +'_p' + \ str(self.pidk_p) + '_i' + \ str(self.pidk_i) + '_d' + \ str(self.pidk_d) + '_' + optargs_str + '.log' self.interface.startLogging(self.logName) """ Stops all logging """ def stopLogging(self): self.interface.stopLogging() ######################## ### 'with' statement ### ######################## def __enter__(self): return self def __exit__(self, exc_type, exc_value, traceback): self.interface.terminate() ##################### ### Motor control ### ##################### """ Sets PID values for all motors """ def setPID(self, p, i, d): self.motorL.setPID(p, i, d) self.motorR.setPID(p, i, d) """ Whether any of the motors are rotating """ def isMoving(self): return self.motorL.isRotating() or self.motorR.isRotating() """ Moves the given distance forwards or backwards """ def move(self, distance): # distance specified in metres wheel = distance * self.movementCoeff self.startLogging(['move', distance]) self.motorL.rotate(self.powerL * wheel) self.motorR.rotate(self.powerR * wheel) self.stopLogging() """ Rotates the whole robot by the given angle clockwise """ def rotate(self, angle): # angle specified in degrees wheel = self.rotatePower * angle * (math.pi/180) * self.botRadius * self.movementCoeff self.startLogging(['turn', angle]) self.motorL.rotate( self.powerL * wheel) self.motorR.rotate(-self.powerR * wheel) self.stopLogging() def drive(self, speedL, speedR): self.motorL.setSpeed(speedL) self.motorR.setSpeed(speedR) def arcPath(self, theta): # theta = 0 returns to straight path # Positive theta => too far to the right => turn left if theta <= 0 : self.powerL = 1 self.powerR = 1 + theta else: self.powerL = 1 - theta self.powerR = 1 """ Blocks the program until the robot stops moving """ def wait(self): while self.isMoving(): time.sleep(self.deltaTime) ###################### ### Sensor control ### ###################### """ Starts the main loop, checking sensors and responding to events until `running` is false """ def mainLoop(self): self.running = True while self.running: self.touchSensorL.check() self.touchSensorR.check() self.ultraSonic.check() self.motorL.check() self.motorR.check() time.sleep(self.deltaTime) def sensorAction(self, params): if(params['position'] != 'either' or not params['down']): return self.wait() # function triggered by the event handler when the touchsensor values are changed. if (self.touchSensorL.getState() == EventState.SENSOR_TOUCH_DOWN) \ and (self.touchSensorR.getState() == EventState.SENSOR_TOUCH_DOWN): self.move(-20) self.wait() self.rotate(self, 90) elif self.touchSensorL.getState() == EventState.SENSOR_TOUCH_DOWN: self.move(-10) self.wait() self.rotate(self, 45) elif self.touchSensorL.getState() == EventState.SENSOR_TOUCH_DOWN: self.move(-10) self.wait() self.rotate(self, -45)