class GyroAndCompassRobot(TwoWheeledRobot): def __init__(self): super(GyroAndCompassRobot, self).__init__() self.gyro = Gyro() self.compass = Compass() self.compassRegulator = PID( getInput = lambda: self.compass.heading, setOutput = lambda x: TwoWheeledRobot.drive(self, speed = 0, steer = x), outputRange = (-100, 100), iLimit = 0.25 ) self.gyroRegulator = PID( getInput = lambda: self.gyro.angularVelocity, setOutput = lambda x: TwoWheeledRobot.drive(self, speed = self.speed, steer = x), outputRange = (-100, 100), iLimit = 0.25 ) #self.regulator.tuneFromZieglerNichols(2.575, 0.698) #Compass PID settings self.compassRegulator.kp = 2.75 # FIRST this number started at 0 and was raised until it started to oscillate self.compassRegulator.ki = 1.5 # THIRD we changed until it stopped dead on. self.compassRegulator.kd = 0.2 # SECOND we changed kd until the amount it overshot by was reduced #Gyro PID settings self.gyroRegulator.kp = 3 # FIRST this number started at 0 and was raised until it started to oscillate self.gyroRegulator.ki = 7.5 # THIRD we changed until it stopped dead on. self.gyroRegulator.kd = 0 # SECOND we changed kd until the amount it overshot by was reduced self.gyroRegulator.target = 0 self.compassRegulator.start() self.gyroRegulator.start() self.speed = 0 @logs.to(logs.movement) def drive(self, speed, steer = 0, regulate = True): """ Drive the robot at a certain speed, by default using the compass to regulate the robot's heading. TODO: Tune PID controller for straight movement. """ if regulate: self.compassRegulator.enabled = False self.speed = speed self.gyro.calibrate() self.gyroRegulator.target = steer self.gyroRegulator.enabled = True else: TwoWheeledRobot.drive(self, speed, steer) @logs.to(logs.movement) def rotateTo(self, angle, tolerance = 5): self.gyroRegulator.enabled = False self.compassRegulator.target = angle self.speed = 0 self.compassRegulator.enabled = True while not self.compassRegulator.onTarget(tolerance=tolerance): time.sleep(0.05) def rotateBy(self, angle, fromTarget = False, tolerance = 5): """ Rotate the robot a certain angle from the direction it is currently facing. Optionally rotate from the last target, preventing errors accumulating """ self.rotateTo((self.compassRegulator.target if fromTarget else self.compass.heading) + angle, tolerance = tolerance) def stop(self): """ Stop the robot, by setting the speed to 0, and disabling regulation """ self.speed = 0 self.gyroRegulator.enabled = False self.compassRegulator.enabled = False super(GyroAndCompassRobot, self).stop()
class CompassRobot(TwoWheeledRobot): def __init__(self): TwoWheeledRobot.__init__(self) self.compass = Compass() self.regulator = PID( getInput = lambda: self.compass.heading, setOutput = lambda x: TwoWheeledRobot.drive(self, speed = self.speed, steer = x), outputRange = (-100, 100), iLimit = 0.25 ) #self.regulator.tuneFromZieglerNichols(2.575, 0.698) #PID settings self.regulator.kp = 2.75 #3.2 #1.500 # FIRST this number started at 0 and was raised until it started to oscillate self.regulator.ki = 1.5 #0.175 # THIRD we changed until it stopped dead on. self.regulator.kd = 0.2 #0.194 #0.080 # SECOND we changed kd until the amount it overshot by was reduced self.regulator.start() self.speed = 0 @property def regulate(self): """Shorthand for enabling and disabling the PID controller""" return self.regulator.enabled @regulate.setter def regulate(self, value): self.regulator.enabled = value def rotateTo(self, heading, tolerance = 2.5): """ Rotate the robot to face the specified heading. Return when within tolerance degrees of the target. Note that this does not stop the motors upon returning """ self.regulate = True; self.speed = 0 self.regulator.target = heading while not self.regulator.onTarget(tolerance=tolerance): time.sleep(0.05) def rotateBy(self, angle, fromTarget = False): """ Rotate the robot a certain angle from the direction it is currently facing. Optionally rotate from the last target, preventing errors accumulating """ self.rotateTo((self.regulator.target if fromTarget else self.compass.heading) + angle) def setSpeed(self, speed): print "deprecated - use drive instead" self.drive(speed, regulate = True) def drive(self, speed, steer = 0, regulate = True): """ Drive the robot at a certain speed, by default using the compass to regulate the robot's heading. TODO: Tune PID controller for straight movement. """ self.regulate = regulate if regulate: self.speed = speed else: TwoWheeledRobot.drive(self, speed, steer) def stop(self): """ Stop the robot, by setting the speed to 0, and disabling regulation """ self.speed = 0 self.regulate = False TwoWheeledRobot.stop(self)
