Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-06-23 06:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Upgrading to 2025.1.0

Browse Source
This commit is contained in:
thenetworkgrinch
2024-12-09 23:26:04 +00:00
parent 9fe6551d88
commit 4bc6978a20
35 changed files with 1902 additions and 1122 deletions
Download Patch File Download Diff File Expand all files Collapse all files

485
swervelib/SwerveDrive.java
View File

@@ -1,5 +1,17 @@
package swervelib;
import static edu.wpi.first.hal.FRCNetComm.tInstances.kRobotDriveSwerve_YAGSL;
import static edu.wpi.first.hal.FRCNetComm.tResourceType.kResourceType_RobotDrive;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.KilogramSquareMeters;
import static edu.wpi.first.units.Units.Kilograms;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Newtons;
import static edu.wpi.first.units.Units.Seconds;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
@@ -16,9 +28,17 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Force;
import edu.wpi.first.units.measure.LinearVelocity;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -29,6 +49,11 @@ import java.util.Map;
import java.util.Optional;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;
import org.ironmaple.simulation.SimulatedArena;
import org.ironmaple.simulation.drivesims.AbstractDriveTrainSimulation;
import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
import org.ironmaple.simulation.drivesims.SwerveModuleSimulation;
import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig;
import swervelib.encoders.CANCoderSwerve;
import swervelib.imu.IMUVelocity;
import swervelib.imu.Pigeon2Swerve;
@@ -39,8 +64,6 @@ import swervelib.parser.Cache;
import swervelib.parser.SwerveControllerConfiguration;
import swervelib.parser.SwerveDriveConfiguration;
import swervelib.simulation.SwerveIMUSimulation;
import swervelib.telemetry.Alert;
import swervelib.telemetry.Alert.AlertType;
import swervelib.telemetry.SwerveDriveTelemetry;
import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
@@ -84,7 +107,7 @@ public class SwerveDrive
private final Alert tunerXRecommendation = new Alert("Swerve Drive",
"Your Swerve Drive is compatible with Tuner X swerve generator, please consider using that instead of YAGSL. More information here!\n" +
"https://pro.docs.ctr-electronics.com/en/latest/docs/tuner/tuner-swerve/index.html",
AlertType.WARNING);
AlertType.kWarning);
/**
* Field object.
*/
@@ -107,20 +130,24 @@ public class SwerveDrive
* Correct for skew that scales with angular velocity in
* {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}
*/
public boolean angularVelocityCorrection = false;
public boolean angularVelocityCorrection = false;
/**
* Correct for skew that scales with angular velocity in
* {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} during auto.
*/
public boolean autonomousAngularVelocityCorrection = false;
public boolean autonomousAngularVelocityCorrection = false;
/**
* Angular Velocity Correction Coefficent (expected values between -0.15 and 0.15).
*/
public double angularVelocityCoefficient = 0;
public double angularVelocityCoefficient = 0;
/**
* Whether to correct heading when driving translationally. Set to true to enable.
*/
public boolean headingCorrection = false;
/**
* MapleSim SwerveDrive.
*/
private SwerveDriveSimulation mapleSimDrive;
/**
* Amount of seconds the duration of the timestep the speeds should be applied for.
*/
@@ -137,7 +164,7 @@ public class SwerveDrive
* Class that calculates robot's yaw velocity using IMU measurements. Used for angularVelocityCorrection in
* {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}.
*/
private IMUVelocity imuVelocity;
private IMUVelocity imuVelocity;
/**
* Simulation of the swerve drive.
*/
@@ -161,7 +188,7 @@ public class SwerveDrive
/**
* Maximum speed of the robot in meters per second.
*/
private double maxSpeedMPS;
private double maxChassisSpeedMPS;
/**
* Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the
@@ -173,24 +200,61 @@ public class SwerveDrive
* @param config The {@link SwerveDriveConfiguration} configuration to base the swerve drive off of.
* @param controllerConfig The {@link SwerveControllerConfiguration} to use when creating the
* {@link SwerveController}.
* @param maxSpeedMPS Maximum speed in meters per second, remember to use {@link Units#feetToMeters(double)} if
* you have feet per second!
* @param maxSpeedMPS Maximum speed of the robot in meters per second, remember to use
* {@link Units#feetToMeters(double)} if you have feet per second!
* @param startingPose Starting {@link Pose2d} on the field.
*/
public SwerveDrive(
SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS)
SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS,
Pose2d startingPose)
{
this.maxSpeedMPS = maxSpeedMPS;
this.maxChassisSpeedMPS = maxSpeedMPS;
swerveDriveConfiguration = config;
swerveController = new SwerveController(controllerConfig);
// Create Kinematics from swerve module locations.
kinematics = new SwerveDriveKinematics(config.moduleLocationsMeters);
odometryThread = new Notifier(this::updateOdometry);
this.swerveModules = config.modules;
// Create an integrator for angle if the robot is being simulated to emulate an IMU
// If the robot is real, instantiate the IMU instead.
if (SwerveDriveTelemetry.isSimulation)
{
simIMU = new SwerveIMUSimulation();
DriveTrainSimulationConfig simulationConfig = DriveTrainSimulationConfig.Default()
.withBumperSize(
Meters.of(config.getTracklength())
.plus(Inches.of(5)),
Meters.of(config.getTrackwidth())
.plus(Inches.of(5)))
.withRobotMass(Kilograms.of(config.physicalCharacteristics.robotMassKg))
.withCustomModuleTranslations(config.moduleLocationsMeters)
.withGyro(config.getGyroSim())
.withSwerveModule(() -> new SwerveModuleSimulation(
config.getDriveMotorSim(),
config.getAngleMotorSim(),
config.physicalCharacteristics.conversionFactor.drive.gearRatio,
config.physicalCharacteristics.conversionFactor.angle.gearRatio,
Volts.of(config.physicalCharacteristics.driveFrictionVoltage),
Volts.of(config.physicalCharacteristics.angleFrictionVoltage),
Inches.of(
config.physicalCharacteristics.conversionFactor.drive.diameter /
2),
KilogramSquareMeters.of(0.02),
config.physicalCharacteristics.wheelGripCoefficientOfFriction));
mapleSimDrive = new SwerveDriveSimulation(simulationConfig, startingPose);
// feed module simulation instances to modules
for (int i = 0; i < swerveModules.length; i++)
{
this.swerveModules[i].configureModuleSimulation(mapleSimDrive.getModules()[i], config.physicalCharacteristics);
}
// register the drivetrain simulation
SimulatedArena.getInstance().addDriveTrainSimulation(mapleSimDrive);
simIMU = new SwerveIMUSimulation(mapleSimDrive.getGyroSimulation());
imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L);
} else
{
@@ -199,19 +263,15 @@ public class SwerveDrive
imuReadingCache = new Cache<>(imu::getRotation3d, 5L);
}
this.swerveModules = config.modules;
// odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions());
swerveDrivePoseEstimator =
new SwerveDrivePoseEstimator(
kinematics,
getYaw(),
getModulePositions(),
new Pose2d(new Translation2d(0, 0),
Rotation2d.fromDegrees(0))); // x,y,heading in radians; Vision measurement std dev, higher=less weight
startingPose); // x,y,heading in radians; Vision measurement std dev, higher=less weight
zeroGyro();
setMaximumSpeed(maxSpeedMPS);
// Initialize Telemetry
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal())
@@ -244,11 +304,15 @@ public class SwerveDrive
}
SwerveDriveTelemetry.measuredStates = new double[SwerveDriveTelemetry.moduleCount * 2];
SwerveDriveTelemetry.desiredStates = new double[SwerveDriveTelemetry.moduleCount * 2];
SwerveDriveTelemetry.desiredStatesObj = new SwerveModuleState[SwerveDriveTelemetry.moduleCount];
SwerveDriveTelemetry.measuredStatesObj = new SwerveModuleState[SwerveDriveTelemetry.moduleCount];
}
odometryThread.startPeriodic(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02);
setOdometryPeriod(SwerveDriveTelemetry.isSimulation ? 0.004 : 0.02);
checkIfTunerXCompatible();
HAL.report(kResourceType_RobotDrive, kRobotDriveSwerve_YAGSL);
}
/**
@@ -300,6 +364,7 @@ public class SwerveDrive
public void setOdometryPeriod(double period)
{
odometryThread.stop();
SimulatedArena.overrideSimulationTimings(Seconds.of(period), 1);
odometryThread.startPeriodic(period);
}
@@ -309,6 +374,7 @@ public class SwerveDrive
public void stopOdometryThread()
{
odometryThread.stop();
SimulatedArena.overrideSimulationTimings(Seconds.of(TimedRobot.kDefaultPeriod), 5);
}
/**
@@ -381,9 +447,8 @@ public class SwerveDrive
public void driveFieldOrientedandRobotOriented(ChassisSpeeds fieldOrientedVelocity,
ChassisSpeeds robotOrientedVelocity)
{
ChassisSpeeds TotalVelocties = ChassisSpeeds.fromFieldRelativeSpeeds(fieldOrientedVelocity, getOdometryHeading())
.plus(robotOrientedVelocity);
drive(TotalVelocties);
fieldOrientedVelocity.toRobotRelativeSpeeds(getOdometryHeading());
drive(fieldOrientedVelocity.plus(robotOrientedVelocity));
}
/**
@@ -393,8 +458,8 @@ public class SwerveDrive
*/
public void driveFieldOriented(ChassisSpeeds velocity)
{
ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
drive(fieldOrientedVelocity);
velocity.toRobotRelativeSpeeds(getOdometryHeading());
drive(velocity);
}
/**
@@ -405,8 +470,8 @@ public class SwerveDrive
*/
public void driveFieldOriented(ChassisSpeeds velocity, Translation2d centerOfRotationMeters)
{
ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
drive(fieldOrientedVelocity, centerOfRotationMeters);
velocity.toRobotRelativeSpeeds(getOdometryHeading());
drive(velocity, centerOfRotationMeters);
}
/**
@@ -454,12 +519,11 @@ public class SwerveDrive
{
// Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if
// necessary.
ChassisSpeeds velocity =
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
translation.getX(), translation.getY(), rotation, getOdometryHeading())
: new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
ChassisSpeeds velocity = new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
if (fieldRelative)
{
velocity.toRobotRelativeSpeeds(getOdometryHeading());
}
drive(velocity, isOpenLoop, centerOfRotationMeters);
}
@@ -483,12 +547,12 @@ public class SwerveDrive
{
// Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if
// necessary.
ChassisSpeeds velocity =
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
translation.getX(), translation.getY(), rotation, getOdometryHeading())
: new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
ChassisSpeeds velocity = new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
if (fieldRelative)
{
velocity.toRobotRelativeSpeeds(getOdometryHeading());
}
drive(velocity, isOpenLoop, new Translation2d());
}
@@ -497,25 +561,27 @@ public class SwerveDrive
* states accordingly. Can use either open-loop or closed-loop velocity control for the wheel velocities. Applies
* heading correction if enabled and necessary.
*
* @param velocity The chassis speeds to set the robot to achieve.
* @param robotRelativeVelocity The chassis speeds to set the robot to achieve.
* @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop.
* @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot.
*/
public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d centerOfRotationMeters)
public void drive(ChassisSpeeds robotRelativeVelocity, boolean isOpenLoop, Translation2d centerOfRotationMeters)
{
velocity = movementOptimizations(velocity, chassisVelocityCorrection, angularVelocityCorrection);
robotRelativeVelocity = movementOptimizations(robotRelativeVelocity,
chassisVelocityCorrection,
angularVelocityCorrection);
// Heading Angular Velocity Deadband, might make a configuration option later.
// Originally made by Team 1466 Webb Robotics.
// Modified by Team 7525 Pioneers and BoiledBurntBagel of 6036
if (headingCorrection)
{
if (Math.abs(velocity.omegaRadiansPerSecond) < HEADING_CORRECTION_DEADBAND
&& (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND
|| Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND))
if (Math.abs(robotRelativeVelocity.omegaRadiansPerSecond) < HEADING_CORRECTION_DEADBAND
&& (Math.abs(robotRelativeVelocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND
|| Math.abs(robotRelativeVelocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND))
{
velocity.omegaRadiansPerSecond =
robotRelativeVelocity.omegaRadiansPerSecond =
swerveController.headingCalculate(getOdometryHeading().getRadians(), lastHeadingRadians);
} else
{
@@ -524,39 +590,30 @@ public class SwerveDrive
}
// Display commanded speed for testing
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.INFO)
{
SmartDashboard.putString("RobotVelocity", velocity.toString());
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
{
SwerveDriveTelemetry.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[0] = velocity.vxMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(velocity.omegaRadiansPerSecond);
SwerveDriveTelemetry.desiredChassisSpeedsObj = robotRelativeVelocity;
}
// Calculate required module states via kinematics
SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(velocity, centerOfRotationMeters);
SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(robotRelativeVelocity,
centerOfRotationMeters);
setRawModuleStates(swerveModuleStates, velocity, isOpenLoop);
setRawModuleStates(swerveModuleStates, robotRelativeVelocity, isOpenLoop);
}
/**
* Set the maximum speeds for desaturation.
*
* @param attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach in meters per
* second.
* @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot can reach while
* translating in meters per second.
* @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can reach while rotating in
* radians per second.
*/
public void setMaximumSpeeds(
double attainableMaxModuleSpeedMetersPerSecond,
double attainableMaxTranslationalSpeedMetersPerSecond,
double attainableMaxRotationalVelocityRadiansPerSecond)
{
setMaximumSpeed(attainableMaxModuleSpeedMetersPerSecond);
this.attainableMaxTranslationalSpeedMetersPerSecond = attainableMaxTranslationalSpeedMetersPerSecond;
this.attainableMaxRotationalVelocityRadiansPerSecond = attainableMaxRotationalVelocityRadiansPerSecond;
this.swerveController.config.maxAngularVelocity = attainableMaxRotationalVelocityRadiansPerSecond;
@@ -564,13 +621,33 @@ public class SwerveDrive
/**
* Get the maximum velocity from {@link SwerveDrive#attainableMaxTranslationalSpeedMetersPerSecond} or
* {@link SwerveDrive#maxSpeedMPS} whichever is higher.
* {@link SwerveDrive#maxChassisSpeedMPS} whichever is higher.
*
* @return Maximum speed in meters/second.
*/
public double getMaximumVelocity()
public double getMaximumChassisVelocity()
{
return Math.max(this.attainableMaxTranslationalSpeedMetersPerSecond, maxSpeedMPS);
return Math.max(this.attainableMaxTranslationalSpeedMetersPerSecond, maxChassisSpeedMPS);
}
/**
* Get the maximum drive velocity of a module as a {@link LinearVelocity}.
*
* @return {@link LinearVelocity} representing the maximum drive speed of a module.
*/
public LinearVelocity getMaximumModuleDriveVelocity()
{
return swerveModules[0].getMaxVelocity();
}
/**
* Get the maximum angular velocity of an azimuth/angle motor in the swerve module.
*
* @return {@link AngularVelocity} of the maximum azimuth/angle motor.
*/
public AngularVelocity getMaximumModuleAngleVelocity()
{
return swerveModules[0].getMaxAngularVelocity();
}
/**
@@ -579,7 +656,7 @@ public class SwerveDrive
*
* @return Maximum angular velocity in radians per second.
*/
public double getMaximumAngularVelocity()
public double getMaximumChassisAngularVelocity()
{
return Math.max(this.attainableMaxRotationalVelocityRadiansPerSecond, swerveController.config.maxAngularVelocity);
}
@@ -595,15 +672,16 @@ public class SwerveDrive
boolean isOpenLoop)
{
// Desaturates wheel speeds
double maxModuleSpeedMPS = getMaximumModuleDriveVelocity().in(MetersPerSecond);
if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0)
{
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, desiredChassisSpeed,
maxSpeedMPS,
maxModuleSpeedMPS,
attainableMaxTranslationalSpeedMetersPerSecond,
attainableMaxRotationalVelocityRadiansPerSecond);
} else
{
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxSpeedMPS);
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxModuleSpeedMPS);
}
// Sets states
@@ -615,17 +693,18 @@ public class SwerveDrive
/**
* Set the module states (azimuth and velocity) directly. Used primarily for auto paths. Does not allow for usage of
* desaturateWheelSpeeds(SwerveModuleState[] moduleStates, ChassisSpeeds desiredChassisSpeed, double
* attainableMaxModuleSpeedMetersPerSecond, double attainableMaxTranslationalSpeedMetersPerSecond, double
* attainableMaxRotationalVelocityRadiansPerSecond)
* {@link SwerveDriveKinematics#desaturateWheelSpeeds(SwerveModuleState[] moduleStates, ChassisSpeeds
* desiredChassisSpeed, double attainableMaxModuleSpeedMetersPerSecond, double
* attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond)}
*
* @param desiredStates A list of SwerveModuleStates to send to the modules.
* @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop.
*/
public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop)
{
double maxModuleSpeedMPS = getMaximumModuleDriveVelocity().in(MetersPerSecond);
desiredStates = kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates));
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxSpeedMPS);
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxModuleSpeedMPS);
// Sets states
for (SwerveModule module : swerveModules)
@@ -635,26 +714,67 @@ public class SwerveDrive
}
/**
* Set chassis speeds with closed-loop velocity control.
* Drive the robot using the {@link SwerveModuleState}, it is recommended to have
* {@link SwerveDrive#setCosineCompensator(boolean)} set to false for this.<br/>
* <p>
*
* @param chassisSpeeds Chassis speeds to set.
* @param robotRelativeVelocity Robot relative {@link ChassisSpeeds}
* @param states Corresponding {@link SwerveModuleState} to use (not checked against the
* {@param robotRelativeVelocity}).
* @param feedforwardForces Feedforward forces generated by set-point generator
*/
public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
public void drive(ChassisSpeeds robotRelativeVelocity, SwerveModuleState[] states, Force[] feedforwardForces)
{
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
{
SwerveDriveTelemetry.desiredChassisSpeedsObj = robotRelativeVelocity;
}
for (SwerveModule module : swerveModules)
{
// from the module configuration, obtain necessary information to calculate feed-forward
// Warning: Will not work well if motor is not what we are expecting.
// Warning: Should replace module.getDriveMotor().simMotor with expected motor type first.
DCMotor driveMotorModel = module.configuration.driveMotor.getSimMotor();
double driveGearRatio = module.configuration.conversionFactors.drive.gearRatio;
double wheelRadiusMeters = Units.inchesToMeters(module.configuration.conversionFactors.drive.diameter) / 2;
chassisSpeeds = movementOptimizations(chassisSpeeds,
autonomousChassisVelocityCorrection,
autonomousAngularVelocityCorrection);
SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond);
setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), chassisSpeeds, false);
// calculation:
double desiredGroundSpeedMPS = states[module.moduleNumber].speedMetersPerSecond;
double feedforwardVoltage = driveMotorModel.getVoltage(
// Since: (1) torque = force * momentOfForce; (2) torque (on wheel) = torque (on motor) * gearRatio
// torque (on motor) = force * wheelRadius / gearRatio
feedforwardForces[module.moduleNumber].in(Newtons) * wheelRadiusMeters / driveGearRatio,
// Since: (1) linear velocity = angularVelocity * wheelRadius; (2) wheelVelocity = motorVelocity / gearRatio
// motorAngularVelocity = linearVelocity / wheelRadius * gearRatio
desiredGroundSpeedMPS / wheelRadiusMeters * driveGearRatio
);
module.setDesiredState(
states[module.moduleNumber],
false,
feedforwardVoltage
);
}
}
/**
* Gets the current pose (position and rotation) of the robot, as reported by odometry.
* Set chassis speeds with closed-loop velocity control.
*
* @param robotRelativeSpeeds Chassis speeds to set.
*/
public void setChassisSpeeds(ChassisSpeeds robotRelativeSpeeds)
{
robotRelativeSpeeds = movementOptimizations(robotRelativeSpeeds,
autonomousChassisVelocityCorrection,
autonomousAngularVelocityCorrection);
SwerveDriveTelemetry.desiredChassisSpeedsObj = robotRelativeSpeeds;
setRawModuleStates(kinematics.toSwerveModuleStates(robotRelativeSpeeds), robotRelativeSpeeds, false);
}
/**
* Gets the measured pose (position and rotation) of the robot, as reported by odometry.
*
* @return The robot's pose
*/
@@ -668,7 +788,38 @@ public class SwerveDrive
}
/**
* Gets the current field-relative velocity (x, y and omega) of the robot
* Gets the maple-sim drivetrain simulation instance This is used to add intake simulation / launch game pieces from
* the robot
*
* @return an optional maple-sim {@link SwerveDriveSimulation} object, or {@link Optional#empty()} when calling from a
* real robot
*/
public Optional<SwerveDriveSimulation> getMapleSimDrive()
{
if (SwerveDriveTelemetry.isSimulation)
{
return Optional.of(mapleSimDrive);
}
return Optional.empty();
}
/**
* Gets the actual pose of the drivetrain during simulation
*
* @return an {@link Optional} {@link Pose2d}, representing the drivetrain pose during simulation, or an empty
* optional when running on real robot
*/
public Optional<Pose2d> getSimulationDriveTrainPose()
{
odometryLock.lock();
Optional<Pose2d> simulationPose = getMapleSimDrive().map(AbstractDriveTrainSimulation::getSimulatedDriveTrainPose);
odometryLock.unlock();
return simulationPose;
}
/**
* Gets the measured field-relative robot velocity (x, y and omega)
*
* @return A ChassisSpeeds object of the current field-relative velocity
*/
@@ -676,10 +827,10 @@ public class SwerveDrive
{
// ChassisSpeeds has a method to convert from field-relative to robot-relative speeds,
// but not the reverse. However, because this transform is a simple rotation, negating the
// angle
// given as the robot angle reverses the direction of rotation, and the conversion is reversed.
return ChassisSpeeds.fromFieldRelativeSpeeds(
kinematics.toChassisSpeeds(getStates()), getOdometryHeading().unaryMinus());
// angle given as the robot angle reverses the direction of rotation, and the conversion is reversed.
ChassisSpeeds robotRelativeSpeeds = kinematics.toChassisSpeeds(getStates());
robotRelativeSpeeds.toFieldRelativeSpeeds(getOdometryHeading());//.unaryMinus());
return robotRelativeSpeeds;
}
/**
@@ -703,8 +854,15 @@ public class SwerveDrive
{
odometryLock.lock();
swerveDrivePoseEstimator.resetPosition(getYaw(), getModulePositions(), pose);
if (SwerveDriveTelemetry.isSimulation)
{
mapleSimDrive.setSimulationWorldPose(pose);
}
odometryLock.unlock();
kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, getYaw()));
ChassisSpeeds robotRelativeSpeeds = new ChassisSpeeds();
robotRelativeSpeeds.toFieldRelativeSpeeds(getYaw());
kinematics.toSwerveModuleStates(robotRelativeSpeeds);
}
/**
@@ -888,48 +1046,6 @@ public class SwerveDrive
}
/**
* Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is used for the
* {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], ChassisSpeeds, boolean)} function and
* {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. This function overrides
* what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates.
*
* @param maximumSpeed Maximum speed for the drive motors in meters / second.
* @param updateModuleFeedforward Update the swerve module feedforward to account for the new maximum speed. This
* should be true unless you have replaced the drive motor feedforward with
* {@link SwerveDrive#replaceSwerveModuleFeedforward(SimpleMotorFeedforward)}
* @param optimalVoltage Optimal voltage to use for the feedforward.
*/
public void setMaximumSpeed(double maximumSpeed, boolean updateModuleFeedforward, double optimalVoltage)
{
maxSpeedMPS = maximumSpeed;
swerveDriveConfiguration.physicalCharacteristics.optimalVoltage = optimalVoltage;
for (SwerveModule module : swerveModules)
{
module.maxSpeed = maximumSpeed;
if (updateModuleFeedforward)
{
module.setFeedforward(SwerveMath.createDriveFeedforward(optimalVoltage,
maximumSpeed,
swerveDriveConfiguration.physicalCharacteristics.wheelGripCoefficientOfFriction));
}
}
}
/**
* Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is used for the
* {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], ChassisSpeeds, boolean)} function and
* {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. This function overrides
* what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates. Overwrites the
* {@link SwerveModule#setFeedforward(SimpleMotorFeedforward)}.
*
* @param maximumSpeed Maximum speed for the drive motors in meters / second.
*/
public void setMaximumSpeed(double maximumSpeed)
{
setMaximumSpeed(maximumSpeed, true, swerveDriveConfiguration.physicalCharacteristics.optimalVoltage);
}
/**
* Point all modules toward the robot center, thus making the robot very difficult to move. Forcing the robot to keep
* the current pose.
@@ -943,10 +1059,7 @@ public class SwerveDrive
new SwerveModuleState(0, swerveModule.configuration.moduleLocation.getAngle());
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
{
SwerveDriveTelemetry.desiredStates[swerveModule.moduleNumber * 2] =
desiredState.angle.getDegrees();
SwerveDriveTelemetry.desiredStates[(swerveModule.moduleNumber * 2) + 1] =
desiredState.speedMetersPerSecond;
SwerveDriveTelemetry.desiredStatesObj[swerveModule.moduleNumber] = desiredState;
}
swerveModule.setDesiredState(desiredState, false, true);
@@ -996,34 +1109,42 @@ public class SwerveDrive
public void updateOdometry()
{
odometryLock.lock();
invalidateCache();
try
{
// Update odometry
swerveDrivePoseEstimator.update(getYaw(), getModulePositions());
if (SwerveDriveTelemetry.isSimulation)
{
try
{
SimulatedArena.getInstance().simulationPeriodic();
} catch (Exception e)
{
DriverStation.reportError("MapleSim error", false);
}
}
// Update angle accumulator if the robot is simulated
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
{
Pose2d[] modulePoses = getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition());
if (SwerveDriveTelemetry.isSimulation)
{
simIMU.updateOdometry(
kinematics,
getStates(),
modulePoses,
field);
}
ChassisSpeeds measuredChassisSpeeds = getRobotVelocity();
SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond;
SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond;
SwerveDriveTelemetry.measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond);
SwerveDriveTelemetry.robotRotation = getOdometryHeading().getDegrees();
SwerveDriveTelemetry.measuredChassisSpeedsObj = getRobotVelocity();
SwerveDriveTelemetry.robotRotationObj = getOdometryHeading();
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal())
{
field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
if (SwerveDriveTelemetry.isSimulation)
{
field.setRobotPose(mapleSimDrive.getSimulatedDriveTrainPose());
field.getObject("OdometryPose").setPose(swerveDrivePoseEstimator.getEstimatedPosition());
field.getObject("XModules").setPoses(getSwerveModulePoses(mapleSimDrive.getSimulatedDriveTrainPose()));
} else
{
field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
}
}
double sumVelocity = 0;
@@ -1039,8 +1160,7 @@ public class SwerveDrive
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
{
SwerveDriveTelemetry.measuredStates[module.moduleNumber * 2] = moduleState.angle.getDegrees();
SwerveDriveTelemetry.measuredStates[(module.moduleNumber * 2) + 1] = moduleState.speedMetersPerSecond;
SwerveDriveTelemetry.measuredStatesObj[module.moduleNumber] = moduleState;
}
}
@@ -1065,6 +1185,18 @@ public class SwerveDrive
odometryLock.unlock();
}
/**
* Invalidate all {@link Cache} object used by the {@link SwerveDrive}
*/
public void invalidateCache()
{
imuReadingCache.update();
for (SwerveModule module : swerveModules)
{
module.invalidateCache();
}
}
/**
* Synchronize angle motor integrated encoders with data from absolute encoders.
*/
@@ -1259,8 +1391,8 @@ public class SwerveDrive
/**
* Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop
*
* @param enable Enable chassis velocity correction, which will use
* {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following.
* @param enable Enable chassis velocity correction, which will use {@link ChassisSpeeds#discretize(double)} with
* the following.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
*/
public void setChassisDiscretization(boolean enable, double dtSeconds)
@@ -1276,10 +1408,10 @@ public class SwerveDrive
* Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop
* and/or auto
*
* @param useInTeleop Enable chassis velocity correction, which will use
* {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in teleop.
* @param useInAuto Enable chassis velocity correction, which will use
* {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in auto.
* @param useInTeleop Enable chassis velocity correction, which will use {@link ChassisSpeeds#discretize(double)} with
* the following in teleop.
* @param useInAuto Enable chassis velocity correction, which will use {@link ChassisSpeeds#discretize(double)} with
* the following in auto.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
*/
public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, double dtSeconds)
@@ -1320,49 +1452,62 @@ public class SwerveDrive
/**
* Correct for skew that worsens as angular velocity increases
*
* @param velocity The chassis speeds to set the robot to achieve.
* @param robotRelativeVelocity The chassis speeds to set the robot to achieve.
* @return {@link ChassisSpeeds} of the robot after angular velocity skew correction.
*/
public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds velocity)
public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds robotRelativeVelocity)
{
var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient);
if (angularVelocity.getRadians() != 0.0)
{
velocity = ChassisSpeeds.fromRobotRelativeSpeeds(
velocity.vxMetersPerSecond,
velocity.vyMetersPerSecond,
velocity.omegaRadiansPerSecond,
getOdometryHeading());
velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading().plus(angularVelocity));
robotRelativeVelocity.toFieldRelativeSpeeds(getOdometryHeading());
robotRelativeVelocity.toRobotRelativeSpeeds(getOdometryHeading().plus(angularVelocity));
}
return velocity;
return robotRelativeVelocity;
}
/**
* Enable desired drive corrections
*
* @param velocity The chassis speeds to set the robot to achieve.
* @param robotRelativeVelocity The chassis speeds to set the robot to achieve.
* @param uesChassisDiscretize Correct chassis velocity using 254's correction.
* @param useAngularVelocitySkewCorrection Use the robot's angular velocity to correct for skew.
* @return The chassis speeds after optimizations.
*/
private ChassisSpeeds movementOptimizations(ChassisSpeeds velocity, boolean uesChassisDiscretize,
private ChassisSpeeds movementOptimizations(ChassisSpeeds robotRelativeVelocity, boolean uesChassisDiscretize,
boolean useAngularVelocitySkewCorrection)
{
if (useAngularVelocitySkewCorrection)
{
velocity = angularVelocitySkewCorrection(velocity);
robotRelativeVelocity = angularVelocitySkewCorrection(robotRelativeVelocity);
}
// Thank you to Jared Russell FRC254 for Open Loop Compensation Code
// https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
if (uesChassisDiscretize)
{
velocity = ChassisSpeeds.discretize(velocity, discretizationdtSeconds);
robotRelativeVelocity.discretize(discretizationdtSeconds);
}
return velocity;
return robotRelativeVelocity;
}
/**
* Convert a {@link ChassisSpeeds} to {@link SwerveModuleState[]} for use elsewhere.
*
* @param robotRelativeVelocity {@link ChassisSpeeds} velocity to use.
* @param optimize Perform chassis velocity correction or angular velocity correction.
* @return {@link SwerveModuleState[]} for use elsewhere.
*/
public SwerveModuleState[] toServeModuleStates(ChassisSpeeds robotRelativeVelocity, boolean optimize)
{
if (optimize)
{
robotRelativeVelocity = movementOptimizations(robotRelativeVelocity,
chassisVelocityCorrection,
angularVelocityCorrection);
}
return kinematics.toSwerveModuleStates(robotRelativeVelocity);
}
}