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)
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