Skip to content

Commit

Permalink
Update documentation
Browse files Browse the repository at this point in the history
  • Loading branch information
@viggy96
viggy96 committed Dec 24, 2024
1 parent 2b00785 commit a382762
Show file tree
Hide file tree
Showing 15 changed files with 1,703 additions and 895 deletions.
10 changes: 6 additions & 4 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,10 +48,12 @@ Custom library for 418 Purple Haze
* Module must be calibrated using calibration tool provided by module manufacturer
* Mixing motor controller vendors within a module is NOT supported (ex. TalonFX for drive, Spark Max for rotation)
* More info [here](src/main/java/org/lasarobotics/drive/swerve/README.md)
* Robot rotation PID
* Automatic module "locking"
* Traction control
* Swerve second order kinematics correction
* Swerve drive parent class
* Robot rotation PID
* Automatic module "locking"
* Traction control
* Swerve second order kinematics correction
* More info [here](src/main/java/org/lasarobotics/drive/swerve/README.md)
* Health monitoring and automatic recovery
* Available in the hardware wrappers and for subsystems
* Configurable input maps
Expand Down
15 changes: 10 additions & 5 deletions src/main/java/org/lasarobotics/drive/RotatePIDController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@
import java.util.HashMap;

import org.apache.commons.math3.analysis.polynomials.PolynomialSplineFunction;
import org.lasarobotics.utils.GlobalConstants;
import org.lasarobotics.utils.PIDConstants;

import edu.wpi.first.math.MathUtil;
Expand All @@ -16,17 +15,19 @@
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Dimensionless;
import edu.wpi.first.units.measure.Time;

/** Rotate PID controller */
public class RotatePIDController {
private final double MIN_DEADBAND = 0.001;
private final double MAX_DEADBAND = 0.2;
private final double FILTER_FACTOR = 1.0 / 3.0;
private final Time MAX_LOOKAHEAD = Units.Seconds.of(2);

private HashMap<Double, Double> m_rotateInputMap = new HashMap<Double, Double>();
private PIDController m_pidController;
private Angle m_rotateScalar;
private int m_lookAhead;
private Time m_lookAhead;
private double m_deadband;
private double m_rotateRequest;
private boolean m_isRotating;
Expand All @@ -39,11 +40,15 @@ public class RotatePIDController {
* @param deadband Controller deadband
* @param lookAhead Number of loops to look ahead by
*/
public RotatePIDController(PolynomialSplineFunction rotateInputCurve, PIDConstants pidf, Angle rotateScalar, Dimensionless deadband, int lookAhead) {
public RotatePIDController(PolynomialSplineFunction rotateInputCurve,
PIDConstants pidf,
Angle rotateScalar,
Dimensionless deadband,
Time lookAhead) {
this.m_pidController = new PIDController(pidf.kP, 0.0, pidf.kD, pidf.period.in(Units.Seconds));
this.m_rotateScalar = rotateScalar;
this.m_deadband = MathUtil.clamp(deadband.in(Units.Percent), MIN_DEADBAND, MAX_DEADBAND);
this.m_lookAhead = lookAhead;
this.m_lookAhead = Units.Seconds.of(MathUtil.clamp(lookAhead.in(Units.Seconds), 0.0, MAX_LOOKAHEAD.in(Units.Seconds)));
this.m_isRotating = false;

// Enable PID position wrapping
Expand Down Expand Up @@ -84,7 +89,7 @@ public AngularVelocity calculate(Angle currentAngle, AngularVelocity rotateRate,
} else {
// When rotation is complete, set setpoint to current angle
if (m_isRotating) {
m_pidController.setSetpoint(currentAngle.in(Units.Degrees) + (rotateRate.in(Units.DegreesPerSecond) * m_lookAhead * GlobalConstants.ROBOT_LOOP_HZ.asPeriod().in(Units.Seconds)));
m_pidController.setSetpoint(currentAngle.in(Units.Degrees) + (rotateRate.in(Units.DegreesPerSecond) * m_lookAhead.in(Units.Seconds)));
m_isRotating = false;
}
}
Expand Down
79 changes: 10 additions & 69 deletions src/main/java/org/lasarobotics/drive/SwervePoseEstimatorService.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,10 +13,8 @@
import java.util.function.Supplier;

import org.lasarobotics.drive.swerve.SwerveModule;
import org.lasarobotics.hardware.IMU;
import org.lasarobotics.hardware.PurpleManager;
import org.lasarobotics.hardware.ctre.Pigeon2;
import org.lasarobotics.hardware.kauailabs.NavX2;
import org.lasarobotics.hardware.reduxrobotics.Canandgyro;
import org.lasarobotics.utils.GlobalConstants;
import org.lasarobotics.vision.AprilTagCamera;
import org.littletonrobotics.junction.AutoLog;
Expand Down Expand Up @@ -55,9 +53,7 @@ public static class SwervePoseEstimatorServiceInputs {
private static final String ESTIMATED_POSES_LOG_ENTRY = "/Vision/EstimatedPoses";

private boolean m_running;
private Supplier<Rotation2d> m_rotation2dSupplier;
private Supplier<AngularVelocity> m_yawRateSupplier;
private Supplier<Boolean> m_imuConnectedSupplier;
private IMU m_imu;
private Supplier<SwerveModulePosition[]> m_swerveModulePositionSupplier;
private Consumer<Pose2d> m_poseResetMethod;
private SwerveDriveKinematics m_kinematics;
Expand All @@ -81,68 +77,13 @@ public static class SwervePoseEstimatorServiceInputs {
* <p>
* This service runs in the background and keeps track of where the robot is on the field
* @param odometryStdDev Standard deviation of wheel odometry measurements
* @param imu NavX2 MXP IMU
* @param imu IMU installed on robot
* @param modules Swerve modules
*/
public SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev, NavX2 imu, SwerveModule... modules) {
this(
odometryStdDev,
() -> imu.getInputs().rotation2d,
() -> imu.getInputs().yawRate,
() -> imu.getInputs().isConnected,
modules
);
}

/**
* Create Swerve Pose Estimator Service
* <p>
* This service runs in the background and keeps track of where the robot is on the field
* @param odometryStdDev Standard deviation of wheel odometry measurements
* @param imu CTRE Pigeon 2.0 IMU
* @param modules Swerve modules
*/
public SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev, Pigeon2 imu, SwerveModule... modules) {
this(
odometryStdDev,
() -> imu.getInputs().rotation2d,
() -> imu.getInputs().yawRate,
() -> true,
modules
);
}

/**
* Create Swerve Pose Estimator Service
* <p>
* This service runs in the background and keeps track of where the robot is on the field
* @param odometryStdDev Standard deviation of wheel odometry measurements
* @param imu Redux Canandgyro IMU
* @param modules Swerve modules
*/
public SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev, Canandgyro imu, SwerveModule... modules) {
this(
odometryStdDev,
() -> imu.getInputs().rotation2d,
() -> imu.getInputs().yawRate,
() -> true,
modules
);
}

private SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev,
Supplier<Rotation2d> rotation2dSupplier,
Supplier<AngularVelocity> yawRateSupplier,
Supplier<Boolean> imuConnectedSupplier,
SwerveModule... modules) {
public SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev, IMU imu, SwerveModule... modules) {
if (modules.length != 4) throw new IllegalArgumentException("Four (4) modules must be used!");
this.m_imu = imu;
this.m_running = false;
// Remember how to get rotation2d from IMU
this.m_rotation2dSupplier = rotation2dSupplier;
// Remember how to get yaw rate from IMU
this.m_yawRateSupplier = yawRateSupplier;
// Remember how to check if IMU is connected
this.m_imuConnectedSupplier = imuConnectedSupplier;

// Get each individual module
var moduleList = Arrays.asList(modules);
Expand Down Expand Up @@ -182,7 +123,7 @@ private SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev,
// Initialise pose estimator
this.m_poseEstimator = new SwerveDrivePoseEstimator(
m_kinematics,
m_rotation2dSupplier.get(),
m_imu.getRotation2d(),
m_swerveModulePositionSupplier.get(),
new Pose2d(),
odometryStdDev,
Expand All @@ -207,7 +148,7 @@ private SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev,
m_currentTimestamp = Logger.getRealTimestamp();

// Check if IMU is connected
boolean isIMUConnected = m_imuConnectedSupplier.get();
boolean isIMUConnected = m_imu.isConnected();

// Get synchronized swerve module positions
var currentModulePositions = m_swerveModulePositionSupplier.get();
Expand All @@ -222,9 +163,9 @@ private SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev,
m_previousModulePositions[i] = currentModulePositions[i];
}
var previousYawAngle = m_yawAngle;
m_yawAngle = (isIMUConnected) ? m_rotation2dSupplier.get() :
m_yawAngle = (isIMUConnected) ? m_imu.getRotation2d() :
m_yawAngle.plus(new Rotation2d(m_kinematics.toTwist2d(moduleDeltas).dtheta));
var yawRate = (isIMUConnected) ? m_yawRateSupplier.get() :
var yawRate = (isIMUConnected) ? m_imu.getYawRate() :
Units.Radians.of(m_yawAngle.minus(previousYawAngle).getRadians()).divide(Units.Microseconds.of(m_currentTimestamp - m_previousTimestamp));

// If no cameras or yaw rate is too high, just update pose based on odometry and exit
Expand Down Expand Up @@ -272,7 +213,7 @@ private SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev,
m_thread.setName(NAME);

// Remember how to reset pose
this.m_poseResetMethod = pose -> m_poseEstimator.resetPosition(m_rotation2dSupplier.get(), m_swerveModulePositionSupplier.get(), pose);
this.m_poseResetMethod = pose -> m_poseEstimator.resetPosition(m_imu.getRotation2d(), m_swerveModulePositionSupplier.get(), pose);

// Register service as pose supplier with PurpleManager for simulation
PurpleManager.setPoseSupplier(this::getPose);
Expand Down
20 changes: 14 additions & 6 deletions src/main/java/org/lasarobotics/drive/TractionControlController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ private State(int value) {

private double m_optimalSlipRatio;
private double m_mass;
private double m_maxLinearSpeed;
private double m_maxLinearVelocity;
private double m_maxAcceleration;
private double m_staticCoF;
private double m_dynamicCoF;
Expand Down Expand Up @@ -94,7 +94,7 @@ public TractionControlController(Dimensionless staticCoF,
this.m_dynamicCoF = dynamicCoF.in(Units.Value);
this.m_optimalSlipRatio = MathUtil.clamp(optimalSlipRatio.in(Units.Value), MIN_SLIP_RATIO, MAX_SLIP_RATIO);
this.m_mass = mass.divide(4).in(Units.Kilograms);
this.m_maxLinearSpeed = Math.floor(maxLinearSpeed.in(Units.MetersPerSecond) * 1000) / 1000;
this.m_maxLinearVelocity = Math.floor(maxLinearSpeed.in(Units.MetersPerSecond) * 1000) / 1000;
this.m_maxAcceleration = m_staticCoF * Units.Gs.one().in(Units.MetersPerSecondPerSecond);
this.m_maxPredictedSlipRatio = (m_maxAcceleration * GlobalConstants.ROBOT_LOOP_HZ.in(Units.Hertz))
/ (m_staticCoF * m_mass * Units.Gs.one().in(Units.MetersPerSecondPerSecond));
Expand Down Expand Up @@ -143,12 +143,12 @@ public LinearVelocity calculate(LinearVelocity velocityRequest,
// Get current slip ratio, and check if slipping
double currentSlipRatio = Math.abs(
inertialVelocity.lte(INERTIAL_VELOCITY_THRESHOLD)
? wheelSpeed.in(Units.MetersPerSecond) / m_maxLinearSpeed
? wheelSpeed.in(Units.MetersPerSecond) / m_maxLinearVelocity
: (Math.abs(wheelSpeed.in(Units.MetersPerSecond)) - inertialVelocity.in(Units.MetersPerSecond)) / inertialVelocity.in(Units.MetersPerSecond)
);
m_isSlipping = m_slippingDebouncer.calculate(
currentSlipRatio > m_optimalSlipRatio
& Math.abs(wheelSpeed.in(Units.MetersPerSecond)) > m_maxLinearSpeed * m_optimalSlipRatio
& Math.abs(wheelSpeed.in(Units.MetersPerSecond)) > m_maxLinearVelocity * m_optimalSlipRatio
& isEnabled()
);

Expand Down Expand Up @@ -178,8 +178,8 @@ & isEnabled()
// Update output, clamping to max linear speed
velocityOutput = Units.MetersPerSecond.of(MathUtil.clamp(
velocityOutput.plus(velocityCorrection).in(Units.MetersPerSecond),
-m_maxLinearSpeed,
+m_maxLinearSpeed
-m_maxLinearVelocity,
+m_maxLinearVelocity
));

return velocityOutput;
Expand All @@ -193,6 +193,14 @@ public boolean isSlipping() {
return m_isSlipping;
}

/**
* Get max linear velocity
* @return Maximum linear velocity
*/
public LinearVelocity getMaxLinearVelocity() {
return Units.MetersPerSecond.of(m_maxLinearVelocity);
}

/**
* Toggle traction control
*/
Expand Down
64 changes: 52 additions & 12 deletions src/main/java/org/lasarobotics/drive/swerve/README.md
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,10 @@
# REV Swerve
# Swerve Drive template

Simply extend the [SwerveDrive](src/main/java/org/lasarobotics/drive/swerve/SwerveDrive.java) for your drive subsystem, and call the `super()` constructor with the appropriate arguments.

Use the [PurpleSwerve](https://github.com/lasarobotics/PurpleSwerve) repo as an example

## REV Swerve

In order to use Spark motor controllers, you MUST have a PWM absolute encoder directly connected to the Spark that controls the rotation motor.

Expand All @@ -13,8 +19,8 @@ Options include:
```
REVSwerveModule lFrontModule = MAXSwerveModule.create(
REVSwerveModule.initializeHardware(
Constants.DriveHardware.LEFT_FRONT_DRIVE_MOTOR_ID,
Constants.DriveHardware.LEFT_FRONT_ROTATE_MOTOR_ID,
new Spark.ID("DriveHardware/Swerve/LeftFront/Drive", 2);
new Spark.ID("DriveHardware/Swerve/LeftFront/Rotate", 3);
MotorKind.NEO_VORTEX,
MotorKind.NEO_550
),
Expand All @@ -34,16 +40,50 @@ REVSwerveModule lFrontModule = MAXSwerveModule.create(
);
```

# CTRE Swerve
## CTRE Swerve

At the current time, the CTRE CANCoder is the ONLY absolute encoder supported.

## MAXSwerve
Use of CANivore is HIGHLY recommended, as adjustment of status frame rates isn't easily possible while protecting the underlying sensor getter methods.

CANivore must be named "canivore" using Phoenix Tuner X.

To create an instance of a module, use the helper class for your module, and call the `create()` method for CTRE.

For example, here's how to create a Swerve X2i module:
```
CTRESwerveModule lFrontModule = SwerveX2Module.create(
CTRESwerveModule.initializeHardware(
new TalonFX.ID("DriveHardware/Swerve/LeftFront/Drive", PhoenixCANBus.CANIVORE, 2);
new TalonFX.ID("DriveHardware/Swerve/LeftFront/Rotate", PhoenixCANBus.CANIVORE, 3);
new CANcoder.ID("DriveHardware/Swerve/LeftFront/Encoder", PhoenixCANBus.CANIVORE, 10);
),
SwerveModule.Location.LeftFront,
SwerveModule.MountOrientation.INVERTED
SwerveX2Module.GearRatio.X4_3,
DriveWheel.create(Units.Inches.of(3.0), Units.Value.of(0.9), Units.Value.of(0.8)),
PIDConstants.of(0.3, 0.0, 0.001, 0.0, 0.0),
FFConstants.of(0.2, 0.0, 0.0, 0.5),
PIDConstants.of(2.0, 0.0, 0.1, 0.0, 0.0),
FFConstants.of(0.2, 0.0, 0.0, 0.01),
Units.Percent.of(8.0),
Units.Pounds.of(135.0),
Units.Meters.of(0.5588),
Units.Meters.of(0.5588),
Units.Seconds.of(3.0),
Units.Amps.of(60.0)
);
```

### MAXSwerve

Only REV Spark motor controllers are currently supported with MAXSwerve.

### Calibration
Modules must be calibrated as per manufacturer instructions: https://docs.revrobotics.com/brushless/spark-max/encoders/maxswerve
#### Calibration
Modules must be calibrated as per manufacturer instructions:
https://docs.revrobotics.com/brushless/spark-max/encoders/maxswerve

### Sample PID values
#### Sample PID values
These values can be a good starting point for MAXSwerve.
Note that rotate feed forward values are only used for simulation.

Expand All @@ -61,14 +101,14 @@ Rotate kS: 0.2

Rotate kA: 0.01

## Swerve X
### Swerve X

### Calibration
#### Calibration
Module must be calibrated as per the manufacturer instructions for each respective module:
* [Swerve X/Xi](https://docs.wcproducts.com/wcp-swervex/overview-and-features/zeroing-module)
* [Swerve X2](https://docs.wcproducts.com/wcp-swerve-x2/overview-and-features/zeroing-module)

## SDS
### SDS

### Calibration
#### Calibration
SDS does not use a "calibration tool" or jig like MAXSwerve or WCP. As such, I have assumed that users will zero the modules to be straight ahead, aligned with the chassis. Therefore the "zero offset" for these modules is zero.
Loading

0 comments on commit a382762

Please sign in to comment.