예제 #1
0
	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)
예제 #2
0
	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