class SwerveModule(): def __init__(self, drive, steer, absolute=True, reverse_drive=False, reverse_steer=False, zero_reading=0, drive_encoder=False, reverse_drive_encoder=False): # Initialise private motor controllers self._drive = CANTalon(drive) self.reverse_drive = reverse_drive self._steer = CANTalon(steer) self.drive_encoder = drive_encoder self._distance_offset = 0 # Offset the drive distance counts # Set up the motor controllers # Different depending on whether we are using absolute encoders or not if absolute: self.counts_per_radian = 1024.0 / (2.0 * math.pi) self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogEncoder) self._steer.changeControlMode(CANTalon.ControlMode.Position) self._steer.reverseSensor(reverse_steer) self._steer.reverseOutput(not reverse_steer) # Read the current encoder position self._steer.setPID(20.0, 0.0, 0.0) # PID values for abs self._offset = zero_reading - 256.0 if reverse_steer: self._offset = -self._offset else: self._steer.changeControlMode(CANTalon.ControlMode.Position) self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder) self._steer.setPID(6.0, 0.0, 0.0) # PID values for rel self.counts_per_radian = (497.0 * (40.0 / 48.0) * 4.0 / (2.0 * math.pi)) self._offset = self.counts_per_radian*2.0*math.pi/4.0 self._steer.setPosition(0.0) if self.drive_encoder: self.drive_counts_per_rev = 80*6.67 self.drive_counts_per_metre = (self.drive_counts_per_rev / (math.pi * 0.1016)) self.drive_max_speed = 570 self._drive.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder) self.changeDriveControlMode(CANTalon.ControlMode.Speed) self._drive.reverseSensor(reverse_drive_encoder) else: self.drive_counts_per_rev = 0.0 self.drive_max_speed = 1.0 self.changeDriveControlMode(CANTalon.ControlMode.PercentVbus) self._drive.setVoltageRampRate(150.0) def changeDriveControlMode(self, control_mode): if self._drive.getControlMode is not control_mode: if control_mode == CANTalon.ControlMode.Speed: self._drive.setPID(1.0, 0.00, 0.0, 1023.0 / self.drive_max_speed) elif control_mode == CANTalon.ControlMode.Position: self._drive.setPID(0.1, 0.0, 0.0, 0.0) self._drive.changeControlMode(control_mode) @property def direction(self): # Read the current direction from the controller setpoint setpoint = self._steer.getSetpoint() return float(setpoint - self._offset) / self.counts_per_radian @property def speed(self): # Read the current speed from the controller setpoint setpoint = self._drive.getSetpoint() return float(setpoint) @property def distance(self): # Read the current position from the encoder and remove the offset return self._drive.getEncPosition() - self._distance_offset def zero_distance(self): self._distance_offset = self._drive.getEncPosition() def steer(self, direction, speed=None): if self.drive_encoder: self.changeDriveControlMode(CANTalon.ControlMode.Speed) else: self.changeDriveControlMode(CANTalon.ControlMode.PercentVbus) # Set the speed and direction of the swerve module # Always choose the direction that minimises movement, # even if this means reversing the drive motor if speed is None: # Force the modules to the direction specified - don't # go to the closest one and reverse. delta = constrain_angle(direction - self.direction) # rescale to +/-pi self._steer.set((self.direction + delta) * self.counts_per_radian + self._offset) self._drive.set(0.0) return if abs(speed) > 0.05: direction = constrain_angle(direction) # rescale to +/-pi current_heading = constrain_angle(self.direction) delta = min_angular_displacement(current_heading, direction) if self.reverse_drive: speed = -speed if abs(constrain_angle(self.direction - direction)) < math.pi / 6.0: self._drive.set(speed*self.drive_max_speed) else: self._drive.set(-speed*self.drive_max_speed) self._steer.set((self.direction + delta) * self.counts_per_radian + self._offset) else: self._drive.set(0.0)