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