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)
class DriveTrain(Subsystem):
"""DriveTrain: is the subsystem responsible for motors and
devices associated with driving subystem.
As a subsystem, we represent the single point of contact
for all drivetrain-related controls. Specifically, commands
that manipulate the drivetrain should 'require' the singleton
instance (via require(robot.driveTrain)). Unless overridden,
our default command, JoystickDriver, is the means by which
driving occurs.
"""
k_minThrottleScale = 0.5
k_defaultDriveSpeed = 100.0 # ~13.0 ft/sec determined experimentally
k_maxDriveSpeed = 150.0 # ~20 ft/sec
k_maxTurnSpeed = 40.0 # ~3-4 ft/sec
k_fastTurn = -1
k_mediumTurn = -.72
k_slowTurn = -.55
k_quadTicksPerWheelRev = 9830
k_wheelDiameterInInches = 14.0
k_wheelCircumferenceInInches = k_wheelDiameterInInches * math.pi
k_quadTicksPerInch = k_quadTicksPerWheelRev / k_wheelCircumferenceInInches
k_turnKp = .1
k_turnKi = 0
k_turnKd = .3
k_turnKf = .001
k_ControlModeSpeed = 0
k_ControlModeVBus = 1
k_ControlModeDisabled = 2
class _turnHelper(PIDOutput):
"""a private helper class for PIDController-based
imu-guided turning.
"""
def __init__(self, driveTrain):
super().__init__()
self.driveTrain = driveTrain
def pidWrite(self, output):
self.driveTrain.turn(output * DriveTrain.k_maxTurnSpeed)
def __init__(self, robot, name=None):
super().__init__(name=name)
self.robot = robot
# STEP 1: instantiate the motor controllers
self.leftMasterMotor = CANTalon(robot.map.k_DtLeftMasterId)
self.leftFollowerMotor = CANTalon(robot.map.k_DtLeftFollowerId)
self.rightMasterMotor = CANTalon(robot.map.k_DtRightMasterId)
self.rightFollowerMotor = CANTalon(robot.map.k_DtRightFollowerId)
# Step 2: Configure the follower Talons: left & right back motors
self.leftFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower)
self.leftFollowerMotor.set(self.leftMasterMotor.getDeviceID())
self.rightFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower)
self.rightFollowerMotor.set(self.rightMasterMotor.getDeviceID())
# STEP 3: Setup speed control mode for the master Talons
self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
# STEP 4: Indicate the feedback device used for closed-loop
# For speed mode, indicate the ticks per revolution
self.leftMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.rightMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.leftMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev)
self.rightMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev)
# STEP 5: Set PID values & closed loop error
self.leftMasterMotor.setPID(0.22, 0, 0)
self.rightMasterMotor.setPID(0.22, 0, 0)
# Add ramp up rate
self.leftMasterMotor.setVoltageRampRate(48.0) # max allowable voltage
# change /sec: reach to
# 12V after 1sec
self.rightMasterMotor.setVoltageRampRate(48.0)
# Add SmartDashboard controls for testing
# Add SmartDashboard live windowS
LiveWindow.addActuator("DriveTrain",
"Left Master %d" % robot.map.k_DtLeftMasterId,
self.leftMasterMotor)
LiveWindow.addActuator("DriveTrain",
"Right Master %d" % robot.map.k_DtRightMasterId,
self.rightMasterMotor)
# init RobotDrive - all driving should occur through its methods
# otherwise we expect motor saftey warnings
self.robotDrive = RobotDrive(self.leftMasterMotor, self.rightMasterMotor)
self.robotDrive.setSafetyEnabled(True)
# init IMU - used for driver & vision feedback as well as for
# some autonomous modes.
self.visionState = self.robot.visionState
self.imu = BNO055()
self.turnPID = PIDController(self.k_turnKp, self.k_turnKd, self.k_turnKf,
self.imu, DriveTrain._turnHelper(self))
self.turnPID.setOutputRange(-1, 1)
self.turnPID.setInputRange(-180, 180)
self.turnPID.setPercentTolerance(2)
self.turnMultiplier = DriveTrain.k_mediumTurn
self.maxSpeed = DriveTrain.k_defaultDriveSpeed
# self.setContinuous() ?
robot.info("Initialized DriveTrain")
def initForCommand(self, controlMode):
self.leftMasterMotor.setEncPosition(0) # async call
self.rightMasterMotor.setEncPosition(0)
self.robotDrive.stopMotor()
self.robotDrive.setMaxOutput(self.maxSpeed)
if controlMode == self.k_ControlModeSpeed:
self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
elif controlMode == self.k_ControlModeVBus:
self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
elif controlMode == self.k_ControlModeDisabled:
# unverified codepath
self.leftMasterMotor.disableControl()
self.rightMasterMotor.disableControl()
else:
self.robot.error("Unexpected control mode")
self.robot.info("driveTrain initDefaultCommand, controlmodes: %d %d" %
(self.leftMasterMotor.getControlMode(),
self.rightMasterMotor.getControlMode()))
def initDefaultCommand(self):
# control modes are integers values:
# 0 percentvbux
# 1 position
# 2 velocity
self.setDefaultCommand(JoystickDriver(self.robot))
self.robotDrive.stopMotor();
def updateDashboard(self):
# TODO: additional items?
SmartDashboard.putNumber("IMU heading", self.getCurrentHeading())
SmartDashboard.putNumber("IMU calibration", self.imu.getCalibration())
def stop(self):
self.robotDrive.stopMotor()
def joystickDrive(self, jsY, jsX, throttle):
""" joystickDrive - called by JoystickDriver command. Inputs
are always on the range [-1, 1]... These values can be scaled
for a better "feel", but should always be in a subset of this
range.
"""
if self.robot.isAutonomous or \
(math.fabs(jx) < 0.075 and math.fabs(jy) < .075):
# joystick dead zone or auto (where we've been scheduled via
# default command machinery)
self.robotDrive.stopMotor()
else:
st = self.scaleThrottle(throttle)
self.robotDrive.arcadeDrive(jsY*self.turnMultiplier, jsX*st)
def drive(self, outputmag, curve):
""" drive - used by drivestraight command..
"""
self.robotDrive.drive(outputmag, curve)
def driveStraight(self, speed):
"""driveStraight: invoked from AutoDriveStraight..
"""
# NB: maxOuput isn't applied via set so
# speed is supposedly measured in rpm but this depends on our
# initialization establishing encoding ticks per revolution.
# This is approximate so we rely on the observed values above.
# (DEFAULT_SPEED_MAX_OUTPUT)
if 0:
self.leftMasterMotor.set(speed)
self.rightMasterMotor.set(speed)
else:
# TODO: validate this codepath
moveValue = speed / self.maxSpeed
rotateValue = 0
self.robotDrive.arcadeDrive(moveValue, rotateValue)
def startAutoTurn(self, degrees):
self.robot.info("start autoturn from %d to %d" %
(self.getHeading(), degrees))
self.turnPID.reset()
self.turnPID.setSetpoint(degrees)
self.turnPID.enable()
def endAutoTurn(self):
if self.turnPID.isEnabled():
self.turnPID.disable()
def isAutoTurning(self):
return self.turnPID.isEnabled()
def isAutoTurnFinished(self):
return self.turnPID.onTarget()
def turn(self, speed):
"""turn takes a speed, not an angle...
A negative speed is interpretted as turn left.
Note that we bypass application of setMaxOutput Which
only applies to calls to robotDrive.
"""
# In order to turn left, we want the right wheels to go
# forward and left wheels to go backward (cf: tankDrive)
# Oddly, our right master motor is reversed, so we compensate here.
# speed < 0: turn left: rightmotor(negative) (forward),
# leftmotor(negative) (backward)
# speed > 0: turn right: rightmotor(positive) (backward)
# leftmotor(positive) (forward)
if 0:
self.leftMasterMotor.set(speed)
self.rightMasterMotor.set(speed)
else:
# TODO: validate this codepath
moveValue = 0
rotateValue = speed / self.maxSpeed
self.robotDrive.arcadeDrive(moveValue, rotateValue)
def trackVision(self):
"""presumed called by either autonomous or teleop commands to
allow the vision subsystem to guide the robot
"""
if self.visionState.DriveLockedOnTarget:
self.stop()
else:
if self.isAutoTurning():
if self.isAutoTurnFinished():
self.endAutoTurn()
self.visionState.DriveLockedOnTarget = True
else:
# setup for an auto-turn
h = self.getCurrentHeading()
tg = self.getTargetHeading(h)
self.startAutoTurn(tg)
def getCurrentHeading(self):
""" getCurrentHeading returns a value between -180 and 180
"""
return math.degrees(self.imu.getHeading()) # getHeading: -pi, pi
def scaleThrottle(self, rawThrottle):
""" scaleThrottle:
returns a scaled value between MIN_THROTTLE_SCALE and 1.0
MIN_THROTTLE_SCALE must be set to the lowest useful scale value through experimentation
Scale the joystick values by throttle before passing to the driveTrain
+1=bottom position; -1=top position
"""
# Throttle in the range of -1 to 1. We would like to change that to a
# range of MIN_THROTTLE_SCALE to 1. #First, multiply the raw throttle
# value by -1 to reverse it (makes "up" maximum (1), and "down" minimum (-1))
# Then, add 1 to make the range 0-2 rather than -1 to +1
# Then multiply by ((1-k_minThrottleScale)/2) to change the range to 0-(1-k_minThrottleScale)
# Finally add k_minThrottleScale to change the range to k_minThrottleScale to 1
#
# Check the results are in the range of k_minThrottleScale to 1, and clip
# it in case the math went horribly wrong.
result = ((rawThrottle * -1) + 1) * ((1-self.k_minThrottleScale) / 2) + self.k_minThrottleScale
if result < self.k_minThrottleScale:
# Somehow our math was wrong. Our value was too low, so force it to the minimum
result = self.k_mintThrottleScale
elif result > 1:
# Somehow our math was wrong. Our value was too high, so force it to the maximum
result = 1.0
return result
def modifyTurnSpeed(self, speedUp):
if speedUp:
self.turnMultiplier = self.k_mediumTurn
else:
self.turnMultiplier = self.k_slowTurn
def inchesToTicks(self, inches):
return int(self.k_quadTicksPerInch * inches)
def destinationReached(self, distance):
return math.fabs(self.leftMasterMotor.getEncPosition()) >= distance or \
math.fabs(self.rightMasterMotr.getEncPosition()) >= distance
