def __init__(self,
mock_com,
enable_zmq,
grc_uplink_address="tcp://localhost:7002",
grc_downlink_address="tcp://localhost:7003",
uplink_per=0,
downlink_per=0):
self._mock_com = mock_com
self.i2c = I2CMock(mock_com)
import response_frames
from devices import EPS, Comm, AntennaController, Imtq, Gyro, RTCDevice
from devices import BACKUP_ANTENNA_CONTROLLER_ADDRESS, PRIMARY_ANTENNA_CONTROLLER_ADDRESS
self.frame_decoder = response_frames.FrameDecoder(
response_frames.frame_factories)
self.eps = EPS()
self.comm = Comm(self.frame_decoder)
self.transmitter = self.comm.transmitter
self.receiver = self.comm.receiver
self.primary_antenna = AntennaController(
PRIMARY_ANTENNA_CONTROLLER_ADDRESS, "Primary Antenna")
self.backup_antenna = AntennaController(
BACKUP_ANTENNA_CONTROLLER_ADDRESS, "Backup Antenna")
self.imtq = Imtq()
self.gyro = Gyro()
self.rtc = RTCDevice()
self.i2c.add_bus_device(self.eps.controller_a)
self.i2c.add_pld_device(self.eps.controller_b)
self.i2c.add_bus_device(self.transmitter)
self.i2c.add_bus_device(self.receiver)
self.i2c.add_bus_device(self.primary_antenna)
self.i2c.add_pld_device(self.backup_antenna)
self.i2c.add_bus_device(self.imtq)
self.i2c.add_pld_device(self.rtc)
self.i2c.add_pld_device(self.gyro)
if enable_zmq:
self.zmq_adapter = ZeroMQAdapter(
self.comm,
grc_uplink_address=grc_uplink_address,
grc_downlink_address=grc_downlink_address,
uplink_per=uplink_per,
downlink_per=downlink_per)
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
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()
