Adding preliminary basic drivetrain stuff
This commit is contained in:
160
src/main/java/frc/robot/utilities/MAXSwerveModule.java
Normal file
160
src/main/java/frc/robot/utilities/MAXSwerveModule.java
Normal file
@@ -0,0 +1,160 @@
|
||||
package frc.robot.utilities;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.CANSparkLowLevel.MotorType;
|
||||
import com.revrobotics.SparkAbsoluteEncoder.Type;
|
||||
import com.revrobotics.SparkPIDController;
|
||||
import com.revrobotics.AbsoluteEncoder;
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
|
||||
import frc.robot.constants.ModuleConstants;
|
||||
|
||||
public class MAXSwerveModule {
|
||||
private final CANSparkMax m_drivingSparkMax;
|
||||
private final CANSparkMax m_turningSparkMax;
|
||||
|
||||
private final RelativeEncoder m_drivingEncoder;
|
||||
private final AbsoluteEncoder m_turningEncoder;
|
||||
|
||||
private final SparkPIDController m_drivingPIDController;
|
||||
private final SparkPIDController m_turningPIDController;
|
||||
|
||||
private double m_chassisAngularOffset = 0;
|
||||
private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d());
|
||||
|
||||
/**
|
||||
* Constructs a MAXSwerveModule and configures the driving and turning motor,
|
||||
* encoder, and PID controller. This configuration is specific to the REV
|
||||
* MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore
|
||||
* Encoder.
|
||||
*/
|
||||
public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
|
||||
m_drivingSparkMax = new CANSparkMax(drivingCANId, MotorType.kBrushless);
|
||||
m_turningSparkMax = new CANSparkMax(turningCANId, MotorType.kBrushless);
|
||||
|
||||
// Factory reset, so we get the SPARKS MAX to a known state before configuring
|
||||
// them. This is useful in case a SPARK MAX is swapped out.
|
||||
m_drivingSparkMax.restoreFactoryDefaults();
|
||||
m_turningSparkMax.restoreFactoryDefaults();
|
||||
|
||||
// Setup encoders and PID controllers for the driving and turning SPARKS MAX.
|
||||
m_drivingEncoder = m_drivingSparkMax.getEncoder();
|
||||
m_turningEncoder = m_turningSparkMax.getAbsoluteEncoder(Type.kDutyCycle);
|
||||
m_drivingPIDController = m_drivingSparkMax.getPIDController();
|
||||
m_turningPIDController = m_turningSparkMax.getPIDController();
|
||||
m_drivingPIDController.setFeedbackDevice(m_drivingEncoder);
|
||||
m_turningPIDController.setFeedbackDevice(m_turningEncoder);
|
||||
|
||||
// Apply position and velocity conversion factors for the driving encoder. The
|
||||
// native units for position and velocity are rotations and RPM, respectively,
|
||||
// but we want meters and meters per second to use with WPILib's swerve APIs.
|
||||
m_drivingEncoder.setPositionConversionFactor(ModuleConstants.kDrivingEncoderPositionFactor);
|
||||
m_drivingEncoder.setVelocityConversionFactor(ModuleConstants.kDrivingEncoderVelocityFactor);
|
||||
|
||||
// Apply position and velocity conversion factors for the turning encoder. We
|
||||
// want these in radians and radians per second to use with WPILib's swerve
|
||||
// APIs.
|
||||
m_turningEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderPositionFactor);
|
||||
m_turningEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderVelocityFactor);
|
||||
|
||||
// Invert the turning encoder, since the output shaft rotates in the opposite direction of
|
||||
// the steering motor in the MAXSwerve Module.
|
||||
m_turningEncoder.setInverted(ModuleConstants.kTurningEncoderInverted);
|
||||
|
||||
// Enable PID wrap around for the turning motor. This will allow the PID
|
||||
// controller to go through 0 to get to the setpoint i.e. going from 350 degrees
|
||||
// to 10 degrees will go through 0 rather than the other direction which is a
|
||||
// longer route.
|
||||
m_turningPIDController.setPositionPIDWrappingEnabled(true);
|
||||
m_turningPIDController.setPositionPIDWrappingMinInput(ModuleConstants.kTurningEncoderPositionPIDMinInput);
|
||||
m_turningPIDController.setPositionPIDWrappingMaxInput(ModuleConstants.kTurningEncoderPositionPIDMaxInput);
|
||||
|
||||
// Set the PID gains for the driving motor. Note these are example gains, and you
|
||||
// may need to tune them for your own robot!
|
||||
m_drivingPIDController.setP(ModuleConstants.kDrivingP);
|
||||
m_drivingPIDController.setI(ModuleConstants.kDrivingI);
|
||||
m_drivingPIDController.setD(ModuleConstants.kDrivingD);
|
||||
m_drivingPIDController.setFF(ModuleConstants.kDrivingFF);
|
||||
m_drivingPIDController.setOutputRange(ModuleConstants.kDrivingMinOutput,
|
||||
ModuleConstants.kDrivingMaxOutput);
|
||||
|
||||
// Set the PID gains for the turning motor. Note these are example gains, and you
|
||||
// may need to tune them for your own robot!
|
||||
m_turningPIDController.setP(ModuleConstants.kTurningP);
|
||||
m_turningPIDController.setI(ModuleConstants.kTurningI);
|
||||
m_turningPIDController.setD(ModuleConstants.kTurningD);
|
||||
m_turningPIDController.setFF(ModuleConstants.kTurningFF);
|
||||
m_turningPIDController.setOutputRange(ModuleConstants.kTurningMinOutput,
|
||||
ModuleConstants.kTurningMaxOutput);
|
||||
|
||||
m_drivingSparkMax.setIdleMode(ModuleConstants.kDrivingMotorIdleMode);
|
||||
m_turningSparkMax.setIdleMode(ModuleConstants.kTurningMotorIdleMode);
|
||||
m_drivingSparkMax.setSmartCurrentLimit(ModuleConstants.kDrivingMotorCurrentLimit);
|
||||
m_turningSparkMax.setSmartCurrentLimit(ModuleConstants.kTurningMotorCurrentLimit);
|
||||
|
||||
// Save the SPARK MAX configurations. If a SPARK MAX browns out during
|
||||
// operation, it will maintain the above configurations.
|
||||
m_drivingSparkMax.burnFlash();
|
||||
m_turningSparkMax.burnFlash();
|
||||
|
||||
m_chassisAngularOffset = chassisAngularOffset;
|
||||
m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
|
||||
m_drivingEncoder.setPosition(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current state of the module.
|
||||
*
|
||||
* @return The current state of the module.
|
||||
*/
|
||||
public SwerveModuleState getState() {
|
||||
// Apply chassis angular offset to the encoder position to get the position
|
||||
// relative to the chassis.
|
||||
return new SwerveModuleState(m_drivingEncoder.getVelocity(),
|
||||
new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current position of the module.
|
||||
*
|
||||
* @return The current position of the module.
|
||||
*/
|
||||
public SwerveModulePosition getPosition() {
|
||||
// Apply chassis angular offset to the encoder position to get the position
|
||||
// relative to the chassis.
|
||||
return new SwerveModulePosition(
|
||||
m_drivingEncoder.getPosition(),
|
||||
new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the desired state for the module.
|
||||
*
|
||||
* @param desiredState Desired state with speed and angle.
|
||||
*/
|
||||
public void setDesiredState(SwerveModuleState desiredState) {
|
||||
// Apply chassis angular offset to the desired state.
|
||||
SwerveModuleState correctedDesiredState = new SwerveModuleState();
|
||||
correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond;
|
||||
correctedDesiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(m_chassisAngularOffset));
|
||||
|
||||
// Optimize the reference state to avoid spinning further than 90 degrees.
|
||||
SwerveModuleState optimizedDesiredState = SwerveModuleState.optimize(correctedDesiredState,
|
||||
new Rotation2d(m_turningEncoder.getPosition()));
|
||||
|
||||
// Command driving and turning SPARKS MAX towards their respective setpoints.
|
||||
m_drivingPIDController.setReference(optimizedDesiredState.speedMetersPerSecond, CANSparkMax.ControlType.kVelocity);
|
||||
m_turningPIDController.setReference(optimizedDesiredState.angle.getRadians(), CANSparkMax.ControlType.kPosition);
|
||||
|
||||
m_desiredState = desiredState;
|
||||
}
|
||||
|
||||
/** Zeroes all the SwerveModule encoders. */
|
||||
public void resetEncoders() {
|
||||
m_drivingEncoder.setPosition(0);
|
||||
}
|
||||
}
|
||||
89
src/main/java/frc/robot/utilities/SwerveUtils.java
Normal file
89
src/main/java/frc/robot/utilities/SwerveUtils.java
Normal file
@@ -0,0 +1,89 @@
|
||||
package frc.robot.utilities;
|
||||
|
||||
public class SwerveUtils {
|
||||
|
||||
/**
|
||||
* Steps a value towards a target with a specified step size.
|
||||
* @param _current The current or starting value. Can be positive or negative.
|
||||
* @param _target The target value the algorithm will step towards. Can be positive or negative.
|
||||
* @param _stepsize The maximum step size that can be taken.
|
||||
* @return The new value for {@code _current} after performing the specified step towards the specified target.
|
||||
*/
|
||||
public static double StepTowards(double _current, double _target, double _stepsize) {
|
||||
if (Math.abs(_current - _target) <= _stepsize) {
|
||||
return _target;
|
||||
}
|
||||
else if (_target < _current) {
|
||||
return _current - _stepsize;
|
||||
}
|
||||
else {
|
||||
return _current + _stepsize;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Steps a value (angle) towards a target (angle) taking the shortest path with a specified step size.
|
||||
* @param _current The current or starting angle (in radians). Can lie outside the 0 to 2*PI range.
|
||||
* @param _target The target angle (in radians) the algorithm will step towards. Can lie outside the 0 to 2*PI range.
|
||||
* @param _stepsize The maximum step size that can be taken (in radians).
|
||||
* @return The new angle (in radians) for {@code _current} after performing the specified step towards the specified target.
|
||||
* This value will always lie in the range 0 to 2*PI (exclusive).
|
||||
*/
|
||||
public static double StepTowardsCircular(double _current, double _target, double _stepsize) {
|
||||
_current = WrapAngle(_current);
|
||||
_target = WrapAngle(_target);
|
||||
|
||||
double stepDirection = Math.signum(_target - _current);
|
||||
double difference = Math.abs(_current - _target);
|
||||
|
||||
if (difference <= _stepsize) {
|
||||
return _target;
|
||||
}
|
||||
else if (difference > Math.PI) { //does the system need to wrap over eventually?
|
||||
//handle the special case where you can reach the target in one step while also wrapping
|
||||
if (_current + 2*Math.PI - _target < _stepsize || _target + 2*Math.PI - _current < _stepsize) {
|
||||
return _target;
|
||||
}
|
||||
else {
|
||||
return WrapAngle(_current - stepDirection * _stepsize); //this will handle wrapping gracefully
|
||||
}
|
||||
|
||||
}
|
||||
else {
|
||||
return _current + stepDirection * _stepsize;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Finds the (unsigned) minimum difference between two angles including calculating across 0.
|
||||
* @param _angleA An angle (in radians).
|
||||
* @param _angleB An angle (in radians).
|
||||
* @return The (unsigned) minimum difference between the two angles (in radians).
|
||||
*/
|
||||
public static double AngleDifference(double _angleA, double _angleB) {
|
||||
double difference = Math.abs(_angleA - _angleB);
|
||||
return difference > Math.PI? (2 * Math.PI) - difference : difference;
|
||||
}
|
||||
|
||||
/**
|
||||
* Wraps an angle until it lies within the range from 0 to 2*PI (exclusive).
|
||||
* @param _angle The angle (in radians) to wrap. Can be positive or negative and can lie multiple wraps outside the output range.
|
||||
* @return An angle (in radians) from 0 and 2*PI (exclusive).
|
||||
*/
|
||||
public static double WrapAngle(double _angle) {
|
||||
double twoPi = 2*Math.PI;
|
||||
|
||||
if (_angle == twoPi) { // Handle this case separately to avoid floating point errors with the floor after the division in the case below
|
||||
return 0.0;
|
||||
}
|
||||
else if (_angle > twoPi) {
|
||||
return _angle - twoPi*Math.floor(_angle / twoPi);
|
||||
}
|
||||
else if (_angle < 0.0) {
|
||||
return _angle + twoPi*(Math.floor((-_angle) / twoPi)+1);
|
||||
}
|
||||
else {
|
||||
return _angle;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user