From e342de84a7ada06453b12baf321e29fe7e495d72 Mon Sep 17 00:00:00 2001 From: Geoff Schmit Date: Sat, 16 Sep 2023 15:45:48 -0500 Subject: [PATCH] characterization improvements (#46) * update the "Swerve Drive Characterization" auto mode to use a larger range of voltages and to use the linear least-squares method (Moore-Penrose pseudoinverse) to calculate kS, kV, and kA (fixes update characterization to include kA and span a wider range of voltages #43) * add the "Swerve Rotate Characterization" to characterize the swerve module's rotation mechanism * add characterization constants for Nova credit: @cyandev --- .vscode/settings.json | 3 + .../lib/team254/drivers/TalonFXFactory.java | 2 +- .../lib/team254/drivers/TalonSRXFactory.java | 2 +- .../java/frc/lib/team3061/RobotConfig.java | 28 ++++ .../frc/lib/team3061/swerve/SwerveModule.java | 35 ++++- .../lib/team3061/swerve/SwerveModuleIO.java | 3 + .../swerve/SwerveModuleIOTalonFXPhoenix6.java | 18 ++- src/main/java/frc/robot/RobotContainer.java | 24 +++- .../commands/FeedForwardCharacterization.java | 106 ++++++++++++--- .../frc/robot/configs/NovaRobotConfig.java | 25 +++- .../subsystems/drivetrain/Drivetrain.java | 121 +++++++++++++++--- 11 files changed, 316 insertions(+), 51 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 0aba3bb0..8a031736 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,8 +56,10 @@ "Nivore", "odometry", "openloop", + "Penrose", "photonvision", "PIDF", + "pseudoinverse", "rapidreact", "REVPHJNI", "rezero", @@ -72,6 +74,7 @@ "tunables", "Vandermonde", "VBAT", + "wpilib", "WPILOG", "Xstance", "YUYV" diff --git a/src/main/java/frc/lib/team254/drivers/TalonFXFactory.java b/src/main/java/frc/lib/team254/drivers/TalonFXFactory.java index 7975fba4..dfe7ae51 100644 --- a/src/main/java/frc/lib/team254/drivers/TalonFXFactory.java +++ b/src/main/java/frc/lib/team254/drivers/TalonFXFactory.java @@ -46,7 +46,7 @@ public static class Configuration { SensorInitializationStrategy.BootToZero; public int CONTROL_FRAME_PERIOD_MS = 20; // 10 - public int MOTION_CONTROL_FRAME_PERIOD_MS = 100; + public int MOTION_CONTROL_FRAME_PERIOD_MS = 3; public int GENERAL_STATUS_FRAME_RATE_MS = 10; public int FEEDBACK_STATUS_FRAME_RATE_MS = 49; diff --git a/src/main/java/frc/lib/team254/drivers/TalonSRXFactory.java b/src/main/java/frc/lib/team254/drivers/TalonSRXFactory.java index f86f7589..0e15abdc 100644 --- a/src/main/java/frc/lib/team254/drivers/TalonSRXFactory.java +++ b/src/main/java/frc/lib/team254/drivers/TalonSRXFactory.java @@ -34,7 +34,7 @@ public static class Configuration { public boolean SENSOR_PHASE = false; public int CONTROL_FRAME_PERIOD_MS = 5; - public int MOTION_CONTROL_FRAME_PERIOD_MS = 100; + public int MOTION_CONTROL_FRAME_PERIOD_MS = 3; public int GENERAL_STATUS_FRAME_RATE_MS = 5; public int FEEDBACK_STATUS_FRAME_RATE_MS = 100; public int QUAD_ENCODER_STATUS_FRAME_RATE_MS = 1000; diff --git a/src/main/java/frc/lib/team3061/RobotConfig.java b/src/main/java/frc/lib/team3061/RobotConfig.java index 74cce02d..0a2bfeb7 100644 --- a/src/main/java/frc/lib/team3061/RobotConfig.java +++ b/src/main/java/frc/lib/team3061/RobotConfig.java @@ -65,6 +65,34 @@ public double getSwerveAngleKD() { return 0.0; } + + /* + * Returns the voltage needed to overcome the swerve module's static friction. Defaults to 0. + * + * @return the voltage needed to overcome the swerve module's static friction + */ + public double getSwerveAngleKS() { + return 0.0; + } + + /** + * Returns the voltage needed to hold (or "cruise") at a given constant velocity. Defaults to 0. + * + * @return the voltage needed to hold (or "cruise") at a given constant velocity + */ + public double getSwerveAngleKV() { + return 0.0; + } + + /** + * Returns the voltage needed to induce a given acceleration in the motor shaft. Defaults to 0. + * + * @return the voltage needed to induce a given acceleration in the motor shaft + */ + public double getSwerveAngleKA() { + return 0.0; + } + /** * Returns the proportional constant for the PID controller for the drive motor on the swerve * module. Defaults to 0. diff --git a/src/main/java/frc/lib/team3061/swerve/SwerveModule.java b/src/main/java/frc/lib/team3061/swerve/SwerveModule.java index 59e79445..d1d83964 100644 --- a/src/main/java/frc/lib/team3061/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/team3061/swerve/SwerveModule.java @@ -4,6 +4,8 @@ package frc.lib.team3061.swerve; +import static frc.robot.Constants.*; + import com.ctre.phoenix6.StatusSignal; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -21,6 +23,7 @@ public class SwerveModule { private int moduleNumber; private double lastAngle; private double maxVelocity; + private double lastAngleMotorVelocity = 0.0; private static final String SUBSYSTEM_NAME = "Swerve"; private static final boolean DEBUGGING = false; @@ -94,12 +97,22 @@ public void setDesiredState( * * @param voltage the specified voltage for the drive motor */ - public void setVoltageForCharacterization(double voltage) { + public void setVoltageForDriveCharacterization(double voltage) { io.setAnglePosition(0.0); lastAngle = 0.0; io.setDriveMotorPercentage(voltage / 12.0); } + /** + * Set the angle motor to the specified voltage. This is only used for characterization via the + * FeedForwardCharacterization command. + * + * @param voltage the specified voltage for the angle motor + */ + public void setVoltageForRotateCharacterization(double voltage) { + io.setAngleMotorPercentage(voltage / 12.0); + } + /** * Get the current state of this swerve module. * @@ -146,6 +159,7 @@ public double getDriveCurrent() { *

This method must be invoked by the drivetrain subsystem's periodic method. */ public void updateAndProcessInputs() { + this.lastAngleMotorVelocity = inputs.angleVelocityRevPerMin; io.updateInputs(inputs); Logger.getInstance().processInputs("Mod" + moduleNumber, inputs); } @@ -168,6 +182,25 @@ public void setAngleBrakeMode(boolean enable) { io.setAngleBrakeMode(enable); } + /** + * Get the velocity of the angle motor in radians per second. + * + * @return the velocity of the angle motor in radians per second + */ + public double getAngleMotorVelocity() { + return inputs.angleVelocityRevPerMin * 2.0 * Math.PI / 60.0; + } + + /** + * Get the acceleration of the angle motor in radians per second^2. + * + * @return the acceleration of the angle motor in radians per second^2 + */ + public double getAngleMotorAcceleration() { + return ((inputs.angleVelocityRevPerMin - this.lastAngleMotorVelocity) / 60.0 * 2.0 * Math.PI) + / LOOP_PERIOD_SECS; + } + /** * Returns a list of status signals for the swerve module related to odometry. This can be used to * synchronize the gyro and swerve modules to improve the accuracy of pose estimation. diff --git a/src/main/java/frc/lib/team3061/swerve/SwerveModuleIO.java b/src/main/java/frc/lib/team3061/swerve/SwerveModuleIO.java index 976b5b42..619f8326 100644 --- a/src/main/java/frc/lib/team3061/swerve/SwerveModuleIO.java +++ b/src/main/java/frc/lib/team3061/swerve/SwerveModuleIO.java @@ -34,6 +34,9 @@ public default void updateInputs(SwerveModuleIOInputs inputs) {} /** Run the drive motor at the specified percentage of full power. */ public default void setDriveMotorPercentage(double percentage) {} + /** Run the angle motor at the specified percentage of full power. */ + public default void setAngleMotorPercentage(double percentage) {} + /** Run the drive motor at the specified velocity. */ public default void setDriveVelocity(double velocity) {} diff --git a/src/main/java/frc/lib/team3061/swerve/SwerveModuleIOTalonFXPhoenix6.java b/src/main/java/frc/lib/team3061/swerve/SwerveModuleIOTalonFXPhoenix6.java index 5c3d8cb6..4da44ef3 100644 --- a/src/main/java/frc/lib/team3061/swerve/SwerveModuleIOTalonFXPhoenix6.java +++ b/src/main/java/frc/lib/team3061/swerve/SwerveModuleIOTalonFXPhoenix6.java @@ -72,6 +72,7 @@ public class SwerveModuleIOTalonFXPhoenix6 implements SwerveModuleIO { private VoltageOut driveVoltageRequest; private VelocityVoltage driveVelocityRequest; + private VoltageOut angleVoltageRequest; private PositionVoltage anglePositionRequest; private StatusSignal anglePositionStatusSignal; @@ -178,6 +179,8 @@ private void configAngleMotor(int angleMotorID, int canCoderID) { config.Slot0.kP = turnKp.get(); config.Slot0.kI = turnKi.get(); config.Slot0.kD = turnKd.get(); + config.Slot0.kS = RobotConfig.getInstance().getSwerveAngleKS(); + config.Slot0.kV = RobotConfig.getInstance().getSwerveAngleKV(); config.ClosedLoopGeneral.ContinuousWrap = true; @@ -202,6 +205,8 @@ private void configAngleMotor(int angleMotorID, int canCoderID) { this.angleVelocityStatusSignal = this.angleMotor.getVelocity(); this.anglePositionErrorStatusSignal = this.angleMotor.getClosedLoopError(); + this.angleVoltageRequest = new VoltageOut(0.0); + this.angleVoltageRequest.EnableFOC = RobotConfig.getInstance().getPhoenix6Licensed(); this.anglePositionRequest = new PositionVoltage(0.0).withSlot(0); this.anglePositionRequest.EnableFOC = RobotConfig.getInstance().getPhoenix6Licensed(); } @@ -341,6 +346,12 @@ public void setDriveMotorPercentage(double percentage) { this.driveMotor.setControl(driveVoltageRequest.withOutput(percentage * 12.0)); } + /** Run the angle motor at the specified percentage of full power. */ + @Override + public void setAngleMotorPercentage(double percentage) { + this.angleMotor.setControl(angleVoltageRequest.withOutput(percentage * 12.0)); + } + /** Run the drive motor at the specified velocity. */ @Override public void setDriveVelocity(double velocity) { @@ -433,12 +444,13 @@ private void updateSim() { // update the simulated TalonFXs and CANcoder based on the model outputs double turnRPM = turnSim.getAngularVelocityRPM(); - double angleEncoderRPS = turnRPM / 60; + double angleEncoderRPS = turnRPM / 60.0; double angleEncoderRotations = angleEncoderRPS * Constants.LOOP_PERIOD_SECS; this.angleEncoderSimState.addPosition(angleEncoderRotations); - this.angleMotorSimState.addRotorPosition(angleEncoderRotations); - this.angleMotorSimState.setRotorVelocity(angleEncoderRPS); + this.angleEncoderSimState.setVelocity(angleEncoderRPS); + this.angleMotorSimState.addRotorPosition(angleEncoderRotations / angleGearRatio); + this.angleMotorSimState.setRotorVelocity(angleEncoderRPS / angleGearRatio); double driveMPS = driveSim.getOutput(0); double driveMotorRPS = Conversions.mpsToFalconRPS(driveMPS, wheelCircumference, driveGearRatio); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 419dd3f6..4e0c172f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -376,17 +376,33 @@ private void configureAutoCommands() { /************ Drive Characterization ************ * - * useful for characterizing the drivetrain (i.e, determining kS and kV) + * useful for characterizing the swerve modules for driving (i.e, determining kS and kV) * */ autoChooser.addOption( - "Drive Characterization", + "Swerve Drive Characterization", new FeedForwardCharacterization( drivetrain, true, new FeedForwardCharacterizationData("drive"), - drivetrain::runCharacterizationVolts, - drivetrain::getCharacterizationVelocity)); + drivetrain::runDriveCharacterizationVolts, + drivetrain::getDriveCharacterizationVelocity, + drivetrain::getDriveCharacterizationAcceleration)); + + /************ Swerve Rotate Characterization ************ + * + * useful for characterizing the swerve modules for rotating (i.e, determining kS and kV) + * + */ + autoChooser.addOption( + "Swerve Rotate Characterization", + new FeedForwardCharacterization( + drivetrain, + true, + new FeedForwardCharacterizationData("rotate"), + drivetrain::runRotateCharacterizationVolts, + drivetrain::getRotateCharacterizationVelocity, + drivetrain::getRotateCharacterizationAcceleration)); /************ Distance Test ************ * diff --git a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java b/src/main/java/frc/robot/commands/FeedForwardCharacterization.java index a1a5edd7..a776a2b6 100644 --- a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java +++ b/src/main/java/frc/robot/commands/FeedForwardCharacterization.java @@ -11,17 +11,17 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.lib.team6328.util.PolynomialRegression; import java.util.LinkedList; import java.util.List; import java.util.function.BiConsumer; import java.util.function.Consumer; import java.util.function.Supplier; +import org.ejml.simple.SimpleMatrix; -@java.lang.SuppressWarnings({"java:S106"}) +@java.lang.SuppressWarnings({"java:S106", "java:S107"}) public class FeedForwardCharacterization extends CommandBase { private static final double START_DELAY_SECS = 2.0; - private static final double RAMP_RATE_VOLTS_PER_SECOND = 0.05; + private static final double RAMP_RATE_VOLTS_PER_SECOND = 1.75; private final boolean forwards; private final boolean isDrive; @@ -32,6 +32,8 @@ public class FeedForwardCharacterization extends CommandBase { private final BiConsumer voltageConsumerDrive; private final Supplier velocitySupplierPrimary; private final Supplier velocitySupplierSecondary; + private final Supplier accelerationSupplierPrimary; + private final Supplier accelerationSupplierSecondary; private final Timer timer = new Timer(); @@ -43,7 +45,9 @@ public FeedForwardCharacterization( FeedForwardCharacterizationData rightData, BiConsumer voltageConsumer, Supplier leftVelocitySupplier, - Supplier rightVelocitySupplier) { + Supplier rightVelocitySupplier, + Supplier leftAccelerationSupplier, + Supplier rightAccelerationSupplier) { addRequirements(drive); this.forwards = forwards; this.isDrive = true; @@ -53,6 +57,8 @@ public FeedForwardCharacterization( this.voltageConsumerDrive = voltageConsumer; this.velocitySupplierPrimary = leftVelocitySupplier; this.velocitySupplierSecondary = rightVelocitySupplier; + this.accelerationSupplierPrimary = leftAccelerationSupplier; + this.accelerationSupplierSecondary = rightAccelerationSupplier; } /** Creates a new FeedForwardCharacterization for a simple subsystem. */ @@ -61,7 +67,8 @@ public FeedForwardCharacterization( boolean forwards, FeedForwardCharacterizationData data, Consumer voltageConsumer, - Supplier velocitySupplier) { + Supplier velocitySupplier, + Supplier accelerationSupplier) { addRequirements(subsystem); this.forwards = forwards; this.isDrive = false; @@ -71,11 +78,14 @@ public FeedForwardCharacterization( this.voltageConsumerDrive = null; this.velocitySupplierPrimary = velocitySupplier; this.velocitySupplierSecondary = null; + this.accelerationSupplierPrimary = accelerationSupplier; + this.accelerationSupplierSecondary = null; } // Called when the command is initially scheduled. @Override public void initialize() { + System.out.println("STARTED"); timer.reset(); timer.start(); } @@ -97,9 +107,10 @@ public void execute() { } else { voltageConsumerSimple.accept(voltage); } - dataPrimary.add(velocitySupplierPrimary.get(), voltage); + dataPrimary.add(accelerationSupplierPrimary.get(), velocitySupplierPrimary.get(), voltage); if (isDrive) { - dataSecondary.add(velocitySupplierSecondary.get(), voltage); + dataSecondary.add( + accelerationSupplierSecondary.get(), velocitySupplierSecondary.get(), voltage); } } } @@ -107,6 +118,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + System.out.println("ENDED"); if (isDrive) { voltageConsumerDrive.accept(0.0, 0.0); } else { @@ -122,37 +134,97 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + double voltage = + (timer.get() - START_DELAY_SECS) * RAMP_RATE_VOLTS_PER_SECOND * (forwards ? 1 : -1); + return voltage >= 12.0; } public static class FeedForwardCharacterizationData { private final String name; private final List velocityData = new LinkedList<>(); private final List voltageData = new LinkedList<>(); + private final List accelerationData = new LinkedList<>(); public FeedForwardCharacterizationData(String name) { this.name = name; } - public void add(double velocity, double voltage) { + public void add(double acceleration, double velocity, double voltage) { if (Math.abs(velocity) > 1E-4) { + accelerationData.add(Math.abs(acceleration)); velocityData.add(Math.abs(velocity)); voltageData.add(Math.abs(voltage)); } } public void print() { - PolynomialRegression regression = - new PolynomialRegression( - velocityData.stream().mapToDouble(Double::doubleValue).toArray(), - voltageData.stream().mapToDouble(Double::doubleValue).toArray(), - 1); + + /* + * We would prefer to use wpilib Matrix class, due to its dimensional safety, but it doesn't + * support an arbitrary number of row (at least over 20). If it is updated in the future, we + * should refactor this to use it. + */ + + /* + * The Permanent-Magnet DC Motor Feedforward Equation: V = kA * a + kV * v + kS where V is the + * voltage, a is the acceleration, v is the velocity, and kA, kV, and kS are corresponding + * constants. For more information refer to: + * https://docs.wpilib.org/en/latest/docs/software/advanced-controls/introduction/introduction-to-feedforward.html#the-permanent-magnet-dc-motor-feedforward-equation + * + *

To characterize the drivetrain, we incrementally increase the applied voltage while + * gathering data for acceleration and velocity, and then solve for kA, kV, and kS using + * least-squared regression. + * + *

A x = b + * + *

where A is an n x 3 matrix, where the first column is the measured acceleration; the + * second, the velocity; the third, 1. x is a 3 x 1 matrix, where the first row is kA; the + * second, kV; the third, kS. b is an n x 1 matrix, where each row is the applied voltage. + * + *

Given this system of linear equations, we solve for x using the linear least-squares + * method (Moore-Penrose pseudoinverse): + * + *

x = (A^T * A)^-1 * A^T * b + * + *

For more information, refer to: + * https://en.m.wikipedia.org/wiki/Moore–Penrose_inverse#Linear_least-squares + */ + SimpleMatrix aMatrix = new SimpleMatrix(accelerationData.size(), 3); + SimpleMatrix bMatrix = new SimpleMatrix(accelerationData.size(), 1); + SimpleMatrix xMatrix; + + for (int i = 0; i < accelerationData.size(); i++) { + aMatrix.set(i, 0, accelerationData.get(i)); + aMatrix.set(i, 1, velocityData.get(i)); + aMatrix.set(i, 2, 1); + bMatrix.set(i, 0, voltageData.get(i)); + } + + xMatrix = aMatrix.transpose().mult(aMatrix).invert().mult(aMatrix.transpose()).mult(bMatrix); + + /* + * Calculate the mean-squared error. The closer to 0, the better the regression. The residual + * vector is: + * + *

r = b - Ax + * + *

the sum of the squares of the residuals is: + * + *

r * r^T + * + *

which results in a single value (matrix of size 1x1). We divide this by the number of + * data points to get the mean-squared error. + */ + SimpleMatrix residualVector = bMatrix.minus(aMatrix.mult(xMatrix)); + SimpleMatrix residualsSquaredSum = residualVector.mult(residualVector.transpose()); + double meanSquaredError = residualsSquaredSum.get(0, 0) / accelerationData.size(); System.out.println("FF Characterization Results (" + name + "):"); System.out.println("\tCount=" + Integer.toString(velocityData.size()) + ""); - System.out.println(String.format("\tR2=%.5f", regression.R2())); - System.out.println(String.format("\tkS=%.5f", regression.beta(0))); - System.out.println(String.format("\tkV=%.5f", regression.beta(1))); + System.out.println(String.format("\tMSE (V^2)=%.5f", meanSquaredError)); + System.out.println(String.format("\tkS (V)=%.5f", xMatrix.get(2, 0))); + System.out.println(String.format("\tkV (V/(m/s) or V/(rad/s))=%.5f", xMatrix.get(1, 0))); + System.out.println(String.format("\tkA (V/(m/s^2) or V/(rad/s^2))=%.5f", xMatrix.get(0, 0))); } } } diff --git a/src/main/java/frc/robot/configs/NovaRobotConfig.java b/src/main/java/frc/robot/configs/NovaRobotConfig.java index 8c85b340..8505a53d 100644 --- a/src/main/java/frc/robot/configs/NovaRobotConfig.java +++ b/src/main/java/frc/robot/configs/NovaRobotConfig.java @@ -45,14 +45,18 @@ public class NovaRobotConfig extends RobotConfig { private static final double ANGLE_KI = 0.0; private static final double ANGLE_KD = 0.0; + private static final double ANGLE_KS = 0.17853; + private static final double ANGLE_KV = 0.43784; + private static final double ANGLE_KA = 0.0; + /* Drive Motor PID Values */ private static final double DRIVE_KP = 0.2402346041055719; private static final double DRIVE_KI = 0.0; private static final double DRIVE_KD = 0.013212903225806451; - private static final double DRIVE_KS = 0.28006; - private static final double DRIVE_KV = 2.32216; - private static final double DRIVE_KA = 0.12872; + private static final double DRIVE_KS = 0.37182; + private static final double DRIVE_KV = 2.72624; + private static final double DRIVE_KA = 0.0; private static final SwerveType SWERVE_TYPE = SwerveType.MK4I; @@ -125,6 +129,21 @@ public double getSwerveAngleKD() { return ANGLE_KD; } + @Override + public double getSwerveAngleKS() { + return ANGLE_KS; + } + + @Override + public double getSwerveAngleKV() { + return ANGLE_KV; + } + + @Override + public double getSwerveAngleKA() { + return ANGLE_KA; + } + @Override public double getSwerveDriveKP() { return DRIVE_KP; diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 0a93fc0a..e497d4ad 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -96,6 +96,20 @@ public class Drivetrain extends SubsystemBase { new SwerveModulePosition(), new SwerveModulePosition() }; + private final SwerveModuleState[] swerveModuleStates = + new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }; + private final SwerveModuleState[] prevSwerveModuleStates = + new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }; private Pose2d estimatedPoseWithoutGyro = new Pose2d(); private boolean isFieldRelative; @@ -153,7 +167,7 @@ public Drivetrain( this.zeroGyroscope(); - this.isFieldRelative = true; + this.isFieldRelative = false; this.chassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); @@ -376,9 +390,9 @@ public void resetPoseToVision(Supplier poseSupplier) { *

If the drive mode is X, the robot will ignore the specified velocities and turn the swerve * modules into the x-stance orientation. * - *

If the drive mode is CHARACTERIZATION, the robot will ignore the specified velocities and - * run the characterization routine. Refer to the FeedForwardCharacterization command class for - * more information. + *

If the drive mode is SWERVE_DRIVE_CHARACTERIZATION or SWERVE_ROTATE_CHARACTERIZATION, the + * robot will ignore the specified velocities and run the appropriate characterization routine. + * Refer to the FeedForwardCharacterization command class for more information. * * @param xVelocity the desired velocity in the x direction (m/s) * @param yVelocity the desired velocity in the y direction (m/s) @@ -427,18 +441,26 @@ public void drive( Logger.getInstance() .recordOutput("Drivetrain/ChassisSpeedVo", chassisSpeeds.omegaRadiansPerSecond); - SwerveModuleState[] swerveModuleStates = + SwerveModuleState[] newSwerveModuleStates = kinematics.toSwerveModuleStates(chassisSpeeds, centerGravity); SwerveDriveKinematics.desaturateWheelSpeeds( - swerveModuleStates, RobotConfig.getInstance().getRobotMaxVelocity()); + newSwerveModuleStates, RobotConfig.getInstance().getRobotMaxVelocity()); - setSwerveModuleStates(swerveModuleStates, isOpenLoop, false); + setSwerveModuleStates(newSwerveModuleStates, isOpenLoop, false); break; - case CHARACTERIZATION: - // In characterization mode, drive at the specified voltage (and turn to zero degrees) + case SWERVE_DRIVE_CHARACTERIZATION: + // In this characterization mode, drive at the specified voltage (and turn to zero degrees) for (SwerveModule swerveModule : swerveModules) { - swerveModule.setVoltageForCharacterization(characterizationVoltage); + swerveModule.setVoltageForDriveCharacterization(characterizationVoltage); + } + break; + + case SWERVE_ROTATE_CHARACTERIZATION: + // In this characterization mode, rotate the swerve modules at the specified voltage (and + // don't drive) + for (SwerveModule swerveModule : swerveModules) { + swerveModule.setVoltageForRotateCharacterization(characterizationVoltage); } break; @@ -485,9 +507,9 @@ public void periodic() { Logger.getInstance().recordOutput("Drivetrain/AvgDriveCurrent", this.getAverageDriveCurrent()); // update swerve module states and positions - SwerveModuleState[] states = new SwerveModuleState[4]; for (int i = 0; i < 4; i++) { - states[i] = swerveModules[i].getState(); + prevSwerveModuleStates[i] = swerveModuleStates[i]; + swerveModuleStates[i] = swerveModules[i].getState(); prevSwerveModulePositions[i] = swerveModulePositions[i]; swerveModulePositions[i] = swerveModules[i].getPosition(); } @@ -534,7 +556,7 @@ public void periodic() { Logger.getInstance().recordOutput("Odometry/RobotNoGyro", estimatedPoseWithoutGyro); Logger.getInstance().recordOutput("Odometry/Robot", poseEstimatorPose); Logger.getInstance().recordOutput("3DField", new Pose3d(poseEstimatorPose)); - Logger.getInstance().recordOutput("SwerveModuleStates/Measured", states); + Logger.getInstance().recordOutput("SwerveModuleStates/Measured", swerveModuleStates); } /** @@ -739,8 +761,8 @@ public PIDController getAutoThetaController() { * * @param volts the commanded voltage */ - public void runCharacterizationVolts(double volts) { - driveMode = DriveMode.CHARACTERIZATION; + public void runDriveCharacterizationVolts(double volts) { + driveMode = DriveMode.SWERVE_DRIVE_CHARACTERIZATION; characterizationVoltage = volts; // invoke drive which will set the characterization voltage to each module @@ -752,12 +774,68 @@ public void runCharacterizationVolts(double volts) { * * @return the average drive velocity in meters/sec */ - public double getCharacterizationVelocity() { - double driveVelocityAverage = 0.0; - for (SwerveModule swerveModule : swerveModules) { - driveVelocityAverage += swerveModule.getState().speedMetersPerSecond; + public double getDriveCharacterizationVelocity() { + ChassisSpeeds speeds = kinematics.toChassisSpeeds(swerveModuleStates); + return Math.sqrt(Math.pow(speeds.vxMetersPerSecond, 2) + Math.pow(speeds.vyMetersPerSecond, 2)); + } + + /** + * Returns the average drive velocity in meters/sec. + * + * @return the average drive velocity in meters/sec + */ + public double getDriveCharacterizationAcceleration() { + ChassisSpeeds prevSpeeds = kinematics.toChassisSpeeds(prevSwerveModuleStates); + ChassisSpeeds speeds = kinematics.toChassisSpeeds(swerveModuleStates); + return Math.sqrt( + Math.pow((speeds.vxMetersPerSecond - prevSpeeds.vxMetersPerSecond), 2) + + Math.pow((speeds.vyMetersPerSecond - prevSpeeds.vyMetersPerSecond), 2)) + / LOOP_PERIOD_SECS; + } + + /** + * Rotates swerve modules at the commanded voltage. + * + * @param volts the commanded voltage + */ + public void runRotateCharacterizationVolts(double volts) { + driveMode = DriveMode.SWERVE_ROTATE_CHARACTERIZATION; + characterizationVoltage = volts; + + // invoke drive which will set the characterization voltage to each module + drive(0, 0, 0, true, false); + } + + /** + * Returns the average rotational velocity in radians/sec. + * + * @return the average rotational velocity in radians/sec + */ + public double getRotateCharacterizationVelocity() { + double avgVelocity = 0.0; + + for (SwerveModule mod : swerveModules) { + avgVelocity += mod.getAngleMotorVelocity(); } - return driveVelocityAverage / 4.0; + + avgVelocity /= swerveModules.length; + return avgVelocity; + } + + /** + * Returns the average rotational acceleration in radians/sec^2. + * + * @return the average rotational acceleration in radians/sec^2 + */ + public double getRotateCharacterizationAcceleration() { + double avgAcceleration = 0.0; + + for (SwerveModule mod : swerveModules) { + avgAcceleration += mod.getAngleMotorAcceleration(); + } + + avgAcceleration /= swerveModules.length; + return avgAcceleration; } /** @@ -842,6 +920,7 @@ private static ChassisSpeeds convertFromDiscreteChassisSpeedsToContinuous( private enum DriveMode { NORMAL, X, - CHARACTERIZATION + SWERVE_DRIVE_CHARACTERIZATION, + SWERVE_ROTATE_CHARACTERIZATION } }