* Degrees:
*
* 360 / (angleGearRatio * encoderTicksPerRotation)
*
*
* Meters:
*
* (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation)
*
*/
@Override
public void configureIntegratedEncoder(double positionConversionFactor)
{
this.positionConversionFactor = positionConversionFactor;
// Taken from democat's library.
// https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L16
configureCANStatusFrames(250);
}
/**
* Set the CAN status frames.
*
* @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information
*/
public void configureCANStatusFrames(int CANStatus1)
{
motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1);
}
/**
* Set the CAN status frames.
*
* @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information
* @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed Supply Current
* Measurement, Sticky Fault Information
* @param CANStatus3 Quadrature Information
* @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature
* @param CANStatus8 Pulse Width Information
* @param CANStatus10 Motion Profiling/Motion Magic Information
* @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1)
* @param CANStatus13 PID0 (Primary PID) Information
* @param CANStatus14 PID1 (Auxiliary PID) Information
* @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX)
* @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current Measurement
*/
public void configureCANStatusFrames(int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4, int CANStatus8,
int CANStatus10, int CANStatus12, int CANStatus13, int CANStatus14,
int CANStatus21, int CANStatusCurrent)
{
motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1);
motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2);
motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3);
motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4);
motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8);
motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10);
motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12);
motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13);
motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14);
motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21);
motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, CANStatusCurrent);
// TODO: Configure Status Frame 2 thru 21 if necessary
// https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods
}
/**
* Configure the PIDF values for the closed loop controller. 0 is disabled or off.
*
* @param config Configuration class holding the PIDF values.
*/
@Override
public void configurePIDF(PIDFConfig config)
{
configuration.slot0.kP = config.p;
configuration.slot0.kI = config.i;
configuration.slot0.kD = config.d;
configuration.slot0.kF = config.f;
configuration.slot0.integralZone = config.iz;
configuration.slot0.closedLoopPeakOutput = config.output.max;
configChanged = true;
}
/**
* Configure the PID wrapping for the position closed loop controller.
*
* @param minInput Minimum PID input.
* @param maxInput Maximum PID input.
*/
@Override
public void configurePIDWrapping(double minInput, double maxInput)
{
// Do nothing
}
/**
* Set the idle mode.
*
* @param isBrakeMode Set the brake mode.
*/
@Override
public void setMotorBrake(boolean isBrakeMode)
{
motor.setNeutralMode(isBrakeMode ? NeutralMode.Brake : NeutralMode.Coast);
}
/**
* Set the motor to be inverted.
*
* @param inverted State of inversion.
*/
@Override
public void setInverted(boolean inverted)
{
Timer.delay(1);
motor.setInverted(inverted);
}
/**
* Save the configurations from flash to EEPROM.
*/
@Override
public void burnFlash()
{
if (configChanged)
{
motor.configAllSettings(configuration, 250);
configChanged = false;
}
}
/**
* Set the percentage output.
*
* @param percentOutput percent out for the motor controller.
*/
@Override
public void set(double percentOutput)
{
motor.set(percentOutput);
}
/**
* Put an angle within the the 360 deg scope of a reference. For example, given a scope reference of 756 degrees,
* assumes the full scope is (720-1080), and places an angle of 22 degrees into it, returning 742 deg.
*
* @param scopeReference Current Angle (deg)
* @param newAngle Target Angle (deg)
* @return Closest angle within scope (deg)
*/
private double placeInAppropriate0To360Scope(double scopeReference, double newAngle)
{
double lowerBound;
double upperBound;
double lowerOffset = (scopeReference % 360);
// Create the interval from the reference angle.
if (lowerOffset >= 0)
{
lowerBound = scopeReference - lowerOffset;
upperBound = scopeReference + (360 - lowerOffset);
} else
{
upperBound = scopeReference - lowerOffset;
lowerBound = scopeReference - (360 + lowerOffset);
}
// Put the angle in the interval.
while (newAngle < lowerBound)
{
newAngle += 360;
}
while (newAngle > upperBound)
{
newAngle -= 360;
}
// Smooth the transition between interval boundaries.
if (newAngle - scopeReference > 180)
{
newAngle -= 360;
} else if (newAngle - scopeReference < -180)
{
newAngle += 360;
}
return newAngle;
}
/**
* Convert the setpoint into native sensor units.
*
* @param setpoint Setpoint to mutate. In meters per second or degrees.
* @return Setpoint as native sensor units. Encoder ticks per 100ms, or Encoder tick.
*/
public double convertToNativeSensorUnits(double setpoint)
{
setpoint =
isDriveMotor ? setpoint * .1 : placeInAppropriate0To360Scope(getPosition(), setpoint);
return setpoint / positionConversionFactor;
}
/**
* Set the closed loop PID controller reference point.
*
* @param setpoint Setpoint in MPS or Angle in degrees.
* @param feedforward Feedforward in volt-meter-per-second or kV.
*/
@Override
public void setReference(double setpoint, double feedforward)
{
if (SwerveDriveTelemetry.isSimulation)
{
PhysicsSim.getInstance().run();
}
burnFlash();
motor.set(
isDriveMotor ? ControlMode.Velocity : ControlMode.Position,
convertToNativeSensorUnits(setpoint),
DemandType.ArbitraryFeedForward,
feedforward / nominalVoltage);
}
/**
* Get the velocity of the integrated encoder.
*
* @return velocity in Meters Per Second, or Degrees per Second.
*/
@Override
public double getVelocity()
{
return (motor.getSelectedSensorVelocity() * 10) * positionConversionFactor;
}
/**
* Get the position of the integrated encoder.
*
* @return Position in Meters or Degrees.
*/
@Override
public double getPosition()
{
return motor.getSelectedSensorPosition() * positionConversionFactor;
}
/**
* Set the integrated encoder position.
*
* @param position Integrated encoder position. Should be angle in degrees or meters.
*/
@Override
public void setPosition(double position)
{
if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation)
{
position = position < 0 ? (position % 360) + 360 : position; // Fixes initial 360 movement.
motor.setSelectedSensorPosition(position / positionConversionFactor, 0, 250);
}
}
/**
* Set the voltage compensation for the swerve module motor.
*
* @param nominalVoltage Nominal voltage for operation to output to.
*/
@Override
public void setVoltageCompensation(double nominalVoltage)
{
configuration.voltageCompSaturation = nominalVoltage;
configChanged = true;
this.nominalVoltage = nominalVoltage;
}
/**
* Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
* voltage compensation. This is useful to protect the motor from current spikes.
*
* @param currentLimit Current limit in AMPS at free speed.
*/
@Override
public void setCurrentLimit(int currentLimit)
{
configuration.supplyCurrLimit.currentLimit = currentLimit;
configuration.supplyCurrLimit.enable = true;
configChanged = true;
}
/**
* Set the maximum rate the open/closed loop output can change by.
*
* @param rampRate Time in seconds to go from 0 to full throttle.
*/
@Override
public void setLoopRampRate(double rampRate)
{
configuration.closedloopRamp = rampRate;
configuration.openloopRamp = rampRate;
configChanged = true;
}
/**
* Get the motor object from the module.
*
* @return Motor object.
*/
@Override
public Object getMotor()
{
return motor;
}
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*
* @return connected absolute encoder state.
*/
@Override
public boolean isAttachedAbsoluteEncoder()
{
return absoluteEncoder;
}
}