Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-06-19 06:21:40 +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

2
swervelib/SwerveController.java
View File

@@ -227,7 +227,7 @@ public class SwerveController
*
* @param angularVelocity Angular velocity in radians per second.
*/
public void setMaximumAngularVelocity(double angularVelocity)
public void setMaximumChassisAngularVelocity(double angularVelocity)
{
config.maxAngularVelocity = angularVelocity;
}

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);
}
}

35
swervelib/SwerveDriveTest.java
View File

@@ -1,20 +1,20 @@
package swervelib;
import static edu.wpi.first.units.MutableMeasure.mutable;
import static edu.wpi.first.units.Units.Degrees;
import static edu.wpi.first.units.Units.DegreesPerSecond;
import static edu.wpi.first.units.Units.Meter;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Seconds;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Distance;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.MutableMeasure;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.units.Voltage;
import edu.wpi.first.units.measure.MutAngle;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.units.measure.MutDistance;
import edu.wpi.first.units.measure.MutLinearVelocity;
import edu.wpi.first.units.measure.MutVoltage;
import edu.wpi.first.units.measure.Voltage;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.Timer;
@@ -37,23 +37,23 @@ public class SwerveDriveTest
/**
* Tracks the voltage being applied to a motor
*/
private static final MutableMeasure<Voltage> m_appliedVoltage = mutable(Volts.of(0));
private static final MutVoltage m_appliedVoltage = new MutVoltage(0, 0, Volts);
/**
* Tracks the distance travelled of a position motor
*/
private static final MutableMeasure<Distance> m_distance = mutable(Meters.of(0));
private static final MutDistance m_distance = new MutDistance(0, 0, Meter);
/**
* Tracks the velocity of a positional motor
*/
private static final MutableMeasure<Velocity<Distance>> m_velocity = mutable(MetersPerSecond.of(0));
private static final MutLinearVelocity m_velocity = new MutLinearVelocity(0, 9, MetersPerSecond);
/**
* Tracks the rotations of an angular motor
*/
private static final MutableMeasure<Angle> m_anglePosition = mutable(Degrees.of(0));
private static final MutAngle m_anglePosition = new MutAngle(0, 0, Degrees);
/**
* Tracks the velocity of an angular motor
*/
private static final MutableMeasure<Velocity<Angle>> m_angVelocity = mutable(DegreesPerSecond.of(0));
private static final MutAngularVelocity m_angVelocity = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
* Set the angle of the modules to a given {@link Rotation2d}
@@ -214,7 +214,7 @@ public class SwerveDriveTest
Timer.delay(1);
Rotation2d startingAbsoluteEncoderPosition = Rotation2d.fromDegrees(absoluteEncoder.getAbsolutePosition());
double driveEncoderPositionRotations = module.getDriveMotor().getPosition() /
module.configuration.conversionFactors.drive;
module.configuration.conversionFactors.drive.factor;
if (automatic)
{
module.getAngleMotor().setVoltage(volts);
@@ -230,8 +230,9 @@ public class SwerveDriveTest
false);
Timer.delay(60);
}
double couplingRatio = (module.getDriveMotor().getPosition() / module.configuration.conversionFactors.drive) -
driveEncoderPositionRotations;
double couplingRatio =
(module.getDriveMotor().getPosition() / module.configuration.conversionFactors.drive.factor) -
driveEncoderPositionRotations;
DriverStation.reportWarning(module.configuration.name + " Coupling Ratio: " + couplingRatio, false);
couplingRatioSum += couplingRatio;
}
@@ -308,7 +309,7 @@ public class SwerveDriveTest
SwerveDrive swerveDrive, double maxVolts)
{
return new SysIdRoutine(config, new SysIdRoutine.Mechanism(
(Measure<Voltage> voltage) -> {
(Voltage voltage) -> {
SwerveDriveTest.centerModules(swerveDrive);
SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, Math.min(voltage.in(Volts), maxVolts));
},
@@ -380,7 +381,7 @@ public class SwerveDriveTest
SwerveDrive swerveDrive)
{
return new SysIdRoutine(config, new SysIdRoutine.Mechanism(
(Measure<Voltage> voltage) -> {
(Voltage voltage) -> {
SwerveDriveTest.powerAngleMotorsVoltage(swerveDrive, voltage.in(Volts));
SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0);
},

208
swervelib/SwerveModule.java
View File

@@ -1,20 +1,29 @@
package swervelib;
import com.revrobotics.CANSparkMax;
import com.revrobotics.MotorFeedbackSensor;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.units.measure.AngularVelocity;
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.smartdashboard.SmartDashboard;
import swervelib.encoders.SparkMaxEncoderSwerve;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.math.SwerveMath;
import swervelib.motors.SparkMaxBrushedMotorSwerve;
import swervelib.motors.SparkMaxSwerve;
import swervelib.motors.SwerveMotor;
import swervelib.parser.Cache;
import swervelib.parser.PIDFConfig;
import swervelib.parser.SwerveModuleConfiguration;
import swervelib.parser.SwerveModulePhysicalCharacteristics;
import swervelib.simulation.SwerveModuleSimulation;
import swervelib.telemetry.Alert;
import swervelib.telemetry.SwerveDriveTelemetry;
import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
@@ -43,7 +52,7 @@ public class SwerveModule
/**
* Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right.
*/
public final int moduleNumber;
public final int moduleNumber;
/**
* Swerve Motors.
*/
@@ -85,9 +94,13 @@ public class SwerveModule
*/
private final String rawDriveVelName;
/**
* Maximum speed of the drive motors in meters per second.
* Maximum {@link LinearVelocity} for the drive motor of the swerve module.
*/
public double maxSpeed;
private LinearVelocity maxDriveVelocity;
/**
* Maximum {@link AngularVelocity} for the azimuth/angle motor of the swerve module.
*/
private AngularVelocity maxAngularVelocity;
/**
* Feedforward for the drive motor during closed loop control.
*/
@@ -95,7 +108,7 @@ public class SwerveModule
/**
* Anti-Jitter AKA auto-centering disabled.
*/
private boolean antiJitterEnabled = true;
private boolean antiJitterEnabled = true;
/**
* Last swerve module state applied.
*/
@@ -111,15 +124,15 @@ public class SwerveModule
/**
* Encoder synchronization queued.
*/
private boolean synchronizeEncoderQueued = false;
private boolean synchronizeEncoderQueued = false;
/**
* Encoder, Absolute encoder synchronization enabled.
*/
private boolean synchronizeEncoderEnabled = false;
private boolean synchronizeEncoderEnabled = false;
/**
* Encoder synchronization deadband in degrees.
*/
private double synchronizeEncoderDeadband = 3;
private double synchronizeEncoderDeadband = 3;
/**
@@ -127,11 +140,8 @@ public class SwerveModule
*
* @param moduleNumber Module number for kinematics.
* @param moduleConfiguration Module constants containing CAN ID's and offsets.
* @param driveFeedforward Drive motor feedforward created by
* {@link SwerveMath#createDriveFeedforward(double, double, double)}.
*/
public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfiguration,
SimpleMotorFeedforward driveFeedforward)
public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfiguration)
{
// angle = 0;
// speed = 0;
@@ -141,15 +151,15 @@ public class SwerveModule
configuration = moduleConfiguration;
angleOffset = moduleConfiguration.angleOffset;
// Initialize Feedforwards.
driveMotorFeedforward = driveFeedforward;
// Create motors from configuration and reset them to defaults.
angleMotor = moduleConfiguration.angleMotor;
driveMotor = moduleConfiguration.driveMotor;
angleMotor.factoryDefaults();
driveMotor.factoryDefaults();
// Initialize Feedforwards.
driveMotorFeedforward = getDefaultFeedforward();
// Configure voltage comp, current limit, and ramp rate.
angleMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage);
driveMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage);
@@ -166,13 +176,18 @@ public class SwerveModule
absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted);
}
if (SwerveDriveTelemetry.isSimulation)
{
simModule = new SwerveModuleSimulation();
}
// Setup the cache for the absolute encoder position.
absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 15);
absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 20);
// Config angle motor/controller
angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle);
angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor);
angleMotor.configurePIDF(moduleConfiguration.anglePIDF);
angleMotor.configurePIDWrapping(0, 180);
angleMotor.configurePIDWrapping(0, 360);
angleMotor.setInverted(moduleConfiguration.angleMotorInverted);
angleMotor.setMotorBrake(false);
@@ -183,7 +198,7 @@ public class SwerveModule
}
// Config drive motor/controller
driveMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.drive);
driveMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.drive.factor);
driveMotor.configurePIDF(moduleConfiguration.velocityPIDF);
driveMotor.setInverted(moduleConfiguration.driveMotorInverted);
driveMotor.setMotorBrake(true);
@@ -191,13 +206,8 @@ public class SwerveModule
driveMotor.burnFlash();
angleMotor.burnFlash();
drivePositionCache = new Cache<>(driveMotor::getPosition, 15);
driveVelocityCache = new Cache<>(driveMotor::getVelocity, 15);
if (SwerveDriveTelemetry.isSimulation)
{
simModule = new SwerveModuleSimulation();
}
drivePositionCache = new Cache<>(driveMotor::getPosition, 20);
driveVelocityCache = new Cache<>(driveMotor::getVelocity, 20);
// Force a cache update on init.
driveVelocityCache.update();
@@ -210,11 +220,11 @@ public class SwerveModule
noEncoderWarning = new Alert("Motors",
"There is no Absolute Encoder on module #" +
moduleNumber,
Alert.AlertType.WARNING);
AlertType.kWarning);
encoderOffsetWarning = new Alert("Motors",
"Pushing the Absolute Encoder offset to the encoder failed on module #" +
moduleNumber,
Alert.AlertType.WARNING);
AlertType.kWarning);
rawAbsoluteAngleName = "swerve/modules/" + configuration.name + "/Raw Absolute Encoder";
adjAbsoluteAngleName = "swerve/modules/" + configuration.name + "/Adjusted Absolute Encoder";
@@ -224,6 +234,20 @@ public class SwerveModule
rawDriveVelName = "swerve/modules/" + configuration.name + "/Raw Drive Velocity";
}
/**
* Get the default {@link SimpleMotorFeedforward} for the swerve module drive motor.
*
* @return {@link SimpleMotorFeedforward} using motor details.
*/
public SimpleMotorFeedforward getDefaultFeedforward()
{
double nominalVoltage = driveMotor.getSimMotor().nominalVoltageVolts;
double maxDriveSpeedMPS = getMaxVelocity().in(MetersPerSecond);
return SwerveMath.createDriveFeedforward(nominalVoltage,
maxDriveSpeedMPS,
configuration.physicalCharacteristics.wheelGripCoefficientOfFriction);
}
/**
* Set the voltage compensation for the swerve module motor.
*
@@ -361,28 +385,45 @@ public class SwerveModule
*/
public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force)
{
desiredState = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getAbsolutePosition()));
desiredState.optimize(Rotation2d.fromDegrees(getAbsolutePosition()));
// If we are forcing the angle
if (!force && antiJitterEnabled)
{
// Prevents module rotation if speed is less than 1%
SwerveMath.antiJitter(desiredState, lastState, Math.min(maxSpeed, 4));
SwerveMath.antiJitter(desiredState, lastState, Math.min(maxDriveVelocity.in(MetersPerSecond), 4));
}
// Cosine compensation.
double velocity = configuration.useCosineCompensator
? getCosineCompensatedVelocity(desiredState)
: desiredState.speedMetersPerSecond;
LinearVelocity nextVelocity = configuration.useCosineCompensator
? getCosineCompensatedVelocity(desiredState)
: MetersPerSecond.of(desiredState.speedMetersPerSecond);
LinearVelocity curVelocity = MetersPerSecond.of(lastState.speedMetersPerSecond);
desiredState.speedMetersPerSecond = nextVelocity.magnitude();
setDesiredState(desiredState, isOpenLoop, driveMotorFeedforward.calculate(curVelocity, nextVelocity).magnitude());
}
/**
* Set the desired state of the swerve module. <br /><b>WARNING: If you are not using one of the functions from
* {@link SwerveDrive} you may screw up {@link SwerveDrive#kinematics}</b>
*
* @param desiredState Desired swerve module state.
* @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control.
* @param driveFeedforwardVoltage Drive motor controller feedforward as a voltage.
*/
public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
double driveFeedforwardVoltage)
{
if (isOpenLoop)
{
double percentOutput = desiredState.speedMetersPerSecond / maxSpeed;
double percentOutput = desiredState.speedMetersPerSecond / maxDriveVelocity.in(MetersPerSecond);
driveMotor.set(percentOutput);
} else
{
driveMotor.setReference(velocity, driveMotorFeedforward.calculate(velocity));
desiredState.speedMetersPerSecond = velocity;
driveMotor.setReference(desiredState.speedMetersPerSecond, driveFeedforwardVoltage);
}
// Prevent module rotation if angle is the same as the previous angle.
@@ -408,10 +449,10 @@ public class SwerveModule
simModule.updateStateAndPosition(desiredState);
}
// TODO: Change and move to SwerveDriveTelemetry
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
{
SwerveDriveTelemetry.desiredStates[moduleNumber * 2] = desiredState.angle.getDegrees();
SwerveDriveTelemetry.desiredStates[(moduleNumber * 2) + 1] = velocity;
SwerveDriveTelemetry.desiredStatesObj[moduleNumber] = desiredState;
}
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
@@ -429,7 +470,7 @@ public class SwerveModule
* @param desiredState Desired {@link SwerveModuleState} to use.
* @return Cosine compensated velocity in meters/second.
*/
private double getCosineCompensatedVelocity(SwerveModuleState desiredState)
private LinearVelocity getCosineCompensatedVelocity(SwerveModuleState desiredState)
{
double cosineScalar = 1.0;
// Taken from the CTRE SwerveModule class.
@@ -447,7 +488,7 @@ public class SwerveModule
cosineScalar = 1;
}
return desiredState.speedMetersPerSecond * (cosineScalar);
return MetersPerSecond.of(desiredState.speedMetersPerSecond).times(cosineScalar);
}
/**
@@ -518,6 +559,13 @@ public class SwerveModule
*/
public double getRawAbsolutePosition()
{
/* During simulation, when no absolute encoders are available, we return the state from the simulation module instead. */
if (SwerveDriveTelemetry.isSimulation)
{
Rotation2d absolutePosition = simModule.getState().angle;
return absolutePosition.getDegrees();
}
double angle;
if (absoluteEncoder != null)
{
@@ -629,9 +677,9 @@ public class SwerveModule
if (absoluteEncoder != null && angleOffset == configuration.angleOffset)
{
// If the absolute encoder is attached.
if (angleMotor.getMotor() instanceof CANSparkMax)
if (angleMotor instanceof SparkMaxSwerve || angleMotor instanceof SparkMaxBrushedMotorSwerve)
{
if (absoluteEncoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor)
if (absoluteEncoder instanceof SparkMaxEncoderSwerve)
{
angleMotor.setAbsoluteEncoder(absoluteEncoder);
if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset))
@@ -677,6 +725,40 @@ public class SwerveModule
}
}
/**
* Get the maximum module velocity as a {@link LinearVelocity} based on the RPM and gear ratio.
*
* @return {@link LinearVelocity} max velocity of the drive wheel.
*/
public LinearVelocity getMaxVelocity()
{
if (maxDriveVelocity == null)
{
maxDriveVelocity = MetersPerSecond.of(
(RadiansPerSecond.of(driveMotor.getSimMotor().freeSpeedRadPerSec).in(RotationsPerSecond) /
configuration.conversionFactors.drive.gearRatio) *
configuration.conversionFactors.drive.diameter);
}
return maxDriveVelocity;
}
/**
* Get the maximum module angular velocity as a {@link AngularVelocity} based on the RPM and gear ratio.
*
* @return {@link AngularVelocity} max velocity of the angle/azimuth.
*/
public AngularVelocity getMaxAngularVelocity()
{
if (maxAngularVelocity == null)
{
maxAngularVelocity = RotationsPerSecond.of(
RadiansPerSecond.of(angleMotor.getSimMotor().freeSpeedRadPerSec).in(RotationsPerSecond) *
configuration.conversionFactors.angle.gearRatio);
}
return maxAngularVelocity;
}
/**
* Update data sent to {@link SmartDashboard}.
*/
@@ -687,9 +769,43 @@ public class SwerveModule
SmartDashboard.putNumber(rawAbsoluteAngleName, absoluteEncoder.getAbsolutePosition());
}
SmartDashboard.putNumber(rawAngleName, angleMotor.getPosition());
SmartDashboard.putNumber(rawDriveName, driveMotor.getPosition());
SmartDashboard.putNumber(rawDriveVelName, driveMotor.getVelocity());
SmartDashboard.putNumber(rawDriveName, drivePositionCache.getValue());
SmartDashboard.putNumber(rawDriveVelName, driveVelocityCache.getValue());
SmartDashboard.putNumber(adjAbsoluteAngleName, getAbsolutePosition());
SmartDashboard.putNumber(absoluteEncoderIssueName, getAbsoluteEncoderReadIssue() ? 1 : 0);
}
/**
* Invalidate the {@link Cache} objects used by {@link SwerveModule}.
*/
public void invalidateCache()
{
absolutePositionCache.update();
drivePositionCache.update();
driveVelocityCache.update();
}
/**
* Obtains the {@link SwerveModuleSimulation} used in simulation.
*
* @return the module simulation, <b>null</b> if this method is called on a real robot
*/
public SwerveModuleSimulation getSimModule()
{
return simModule;
}
/**
* Configure the {@link SwerveModule#simModule} with the MapleSim
* {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation}
*
* @param swerveModuleSimulation MapleSim {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} to
* configure with.
*/
public void configureModuleSimulation(
org.ironmaple.simulation.drivesims.SwerveModuleSimulation swerveModuleSimulation,
SwerveModulePhysicalCharacteristics physicalCharacteristics)
{
this.simModule.configureSimModule(swerveModuleSimulation, physicalCharacteristics);
}
}

7
swervelib/encoders/AnalogAbsoluteEncoderSwerve.java
View File

@@ -1,8 +1,9 @@
package swervelib.encoders;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.RobotController;
import swervelib.telemetry.Alert;
/**
* Swerve Absolute Encoder for Thrifty Encoders and other analog encoders.
@@ -39,11 +40,11 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder
cannotSetOffset = new Alert(
"Encoders",
"Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(),
Alert.AlertType.WARNING);
AlertType.kWarning);
inaccurateVelocities = new Alert(
"Encoders",
"The Analog Absolute encoder may not report accurate velocities!",
Alert.AlertType.WARNING_TRACE);
AlertType.kWarning);
}
/**

25
swervelib/encoders/CANCoderSwerve.java
View File

@@ -1,15 +1,20 @@
package swervelib.encoders;
import static edu.wpi.first.units.Units.Degrees;
import static edu.wpi.first.units.Units.DegreesPerSecond;
import static edu.wpi.first.units.Units.Rotations;
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.CANcoderConfigurator;
import com.ctre.phoenix6.configs.MagnetSensorConfigs;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
import com.ctre.phoenix6.signals.MagnetHealthValue;
import com.ctre.phoenix6.signals.SensorDirectionValue;
import swervelib.telemetry.Alert;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
/**
* Swerve Absolute Encoder for CTRE CANCoders.
@@ -65,21 +70,21 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
magnetFieldLessThanIdeal = new Alert(
"Encoders",
"CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.",
Alert.AlertType.WARNING);
AlertType.kWarning);
readingFaulty = new Alert(
"Encoders",
"CANCoder " + encoder.getDeviceID() + " reading was faulty.",
Alert.AlertType.WARNING);
AlertType.kWarning);
readingIgnored = new Alert(
"Encoders",
"CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.",
Alert.AlertType.WARNING);
AlertType.kWarning);
cannotSetOffset = new Alert(
"Encoders",
"Failure to set CANCoder "
+ encoder.getDeviceID()
+ " Absolute Encoder Offset",
Alert.AlertType.WARNING);
AlertType.kWarning);
}
/**
@@ -112,7 +117,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
MagnetSensorConfigs magnetSensorConfiguration = new MagnetSensorConfigs();
cfg.refresh(magnetSensorConfiguration);
cfg.apply(magnetSensorConfiguration
.withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
.withAbsoluteSensorDiscontinuityPoint(Rotations.of(1))
.withSensorDirection(inverted ? SensorDirectionValue.Clockwise_Positive
: SensorDirectionValue.CounterClockwise_Positive));
}
@@ -139,7 +144,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
readingFaulty.set(false);
}
StatusSignal<Double> angle = encoder.getAbsolutePosition();
StatusSignal<Angle> angle = encoder.getAbsolutePosition();
// Taken from democat's library.
// Source: https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/CanCoderFactoryBuilder.java#L51-L74
@@ -160,7 +165,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
readingIgnored.set(false);
}
return angle.getValue() * 360;
return angle.getValue().in(Degrees);
}
/**
@@ -213,6 +218,6 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
@Override
public double getVelocity()
{
return encoder.getVelocity().getValue() * 360;
return encoder.getVelocity().getValue().in(DegreesPerSecond);
}
}

5
swervelib/encoders/PWMDutyCycleEncoderSwerve.java
View File

@@ -1,7 +1,8 @@
package swervelib.encoders;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.DutyCycleEncoder;
import swervelib.telemetry.Alert;
/**
* DutyCycle encoders such as "US Digital MA3 with PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag
@@ -42,7 +43,7 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder
inaccurateVelocities = new Alert(
"Encoders",
"The PWM Duty Cycle encoder may not report accurate velocities!",
Alert.AlertType.WARNING_TRACE);
AlertType.kWarning);
}

50
swervelib/encoders/SparkMaxAnalogEncoderSwerve.java
View File

@@ -1,12 +1,15 @@
package swervelib.encoders;
import com.revrobotics.CANSparkMax;
import com.revrobotics.REVLibError;
import com.revrobotics.SparkAnalogSensor;
import com.revrobotics.SparkAnalogSensor.Mode;
import com.revrobotics.spark.SparkAnalogSensor;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import java.util.function.Supplier;
import swervelib.motors.SparkMaxBrushedMotorSwerve;
import swervelib.motors.SparkMaxSwerve;
import swervelib.motors.SwerveMotor;
import swervelib.telemetry.Alert;
/**
* SparkMax absolute encoder, attached through the data port analog pin.
@@ -14,33 +17,38 @@ import swervelib.telemetry.Alert;
public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
{
/**
* {@link swervelib.motors.SparkMaxSwerve} or {@link swervelib.motors.SparkMaxBrushedMotorSwerve} object.
*/
private final SwerveMotor sparkMax;
/**
* The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port.
*/
public SparkAnalogSensor encoder;
public SparkAnalogSensor encoder;
/**
* An {@link Alert} for if there is a failure configuring the encoder.
*/
private Alert failureConfiguring;
private Alert failureConfiguring;
/**
* An {@link Alert} for if the absolute encoder does not support integrated offsets.
*/
private Alert doesNotSupportIntegratedOffsets;
private Alert doesNotSupportIntegratedOffsets;
/**
* Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link CANSparkMax} motor data
* port analog pin.
* Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link SparkMax} motor data port
* analog pin.
*
* @param motor Motor to create the encoder from.
* @param maxVoltage Maximum voltage for analog input reading.
*/
public SparkMaxAnalogEncoderSwerve(SwerveMotor motor, double maxVoltage)
{
if (motor.getMotor() instanceof CANSparkMax)
if (motor.getMotor() instanceof SparkMax)
{
encoder = ((CANSparkMax) motor.getMotor()).getAnalog(Mode.kAbsolute);
encoder.setPositionConversionFactor(360 / maxVoltage);
sparkMax = motor;
encoder = ((SparkMax) motor.getMotor()).getAnalog();
motor.setAbsoluteEncoder(this);
sparkMax.configureIntegratedEncoder(360 / maxVoltage);
} else
{
throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax");
@@ -48,11 +56,11 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
failureConfiguring = new Alert(
"Encoders",
"Failure configuring SparkMax Analog Encoder",
Alert.AlertType.WARNING_TRACE);
AlertType.kWarning);
doesNotSupportIntegratedOffsets = new Alert(
"Encoders",
"SparkMax Analog Sensors do not support integrated offsets",
Alert.AlertType.WARNING_TRACE);
AlertType.kWarning);
}
@@ -99,7 +107,17 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
@Override
public void configure(boolean inverted)
{
encoder.setInverted(inverted);
if (sparkMax instanceof SparkMaxSwerve)
{
SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig();
cfg.analogSensor.inverted(true);
((SparkMaxSwerve) sparkMax).updateConfig(cfg);
} else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
{
SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
cfg.analogSensor.inverted(true);
((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
}
}
/**

68
swervelib/encoders/SparkMaxEncoderSwerve.java
View File

@@ -1,12 +1,16 @@
package swervelib.encoders;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkMax;
import com.revrobotics.REVLibError;
import com.revrobotics.SparkAbsoluteEncoder.Type;
import com.revrobotics.spark.SparkAbsoluteEncoder;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import java.util.function.Supplier;
import swervelib.motors.SparkMaxBrushedMotorSwerve;
import swervelib.motors.SparkMaxSwerve;
import swervelib.motors.SwerveMotor;
import swervelib.telemetry.Alert;
/**
* SparkMax absolute encoder, attached through the data port.
@@ -17,18 +21,23 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
/**
* The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax.
*/
public AbsoluteEncoder encoder;
public SparkAbsoluteEncoder encoder;
/**
* An {@link Alert} for if there is a failure configuring the encoder.
*/
private Alert failureConfiguring;
private Alert failureConfiguring;
/**
* An {@link Alert} for if there is a failure configuring the encoder offset.
*/
private Alert offsetFailure;
private Alert offsetFailure;
/**
* {@link SparkMaxBrushedMotorSwerve} or {@link SparkMaxSwerve} instance.
*/
private SwerveMotor sparkMax;
/**
* Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link CANSparkMax} motor.
* Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link com.revrobotics.spark.SparkMax}
* motor.
*
* @param motor Motor to create the encoder from.
* @param conversionFactor The conversion factor to set if the output is not from 0 to 360.
@@ -38,16 +47,17 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
failureConfiguring = new Alert(
"Encoders",
"Failure configuring SparkMax Analog Encoder",
Alert.AlertType.WARNING_TRACE);
AlertType.kWarning);
offsetFailure = new Alert(
"Encoders",
"Failure to set Absolute Encoder Offset",
Alert.AlertType.WARNING_TRACE);
if (motor.getMotor() instanceof CANSparkMax)
AlertType.kWarning);
if (motor.getMotor() instanceof SparkMax)
{
encoder = ((CANSparkMax) motor.getMotor()).getAbsoluteEncoder(Type.kDutyCycle);
configureSparkMax(() -> encoder.setVelocityConversionFactor(conversionFactor));
configureSparkMax(() -> encoder.setPositionConversionFactor(conversionFactor));
sparkMax = motor;
encoder = ((SparkMax) motor.getMotor()).getAbsoluteEncoder();
motor.setAbsoluteEncoder(this);
motor.configureIntegratedEncoder(conversionFactor);
} else
{
throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax");
@@ -97,7 +107,17 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
@Override
public void configure(boolean inverted)
{
encoder.setInverted(inverted);
if (sparkMax instanceof SparkMaxSwerve)
{
SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig();
cfg.analogSensor.inverted(true);
((SparkMaxSwerve) sparkMax).updateConfig(cfg);
} else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
{
SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
cfg.analogSensor.inverted(true);
((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
}
}
/**
@@ -131,17 +151,19 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
REVLibError error = null;
for (int i = 0; i < maximumRetries; i++)
if (sparkMax instanceof SparkMaxSwerve)
{
error = encoder.setZeroOffset(offset);
if (error == REVLibError.kOk)
{
return true;
}
SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig();
cfg.absoluteEncoder.zeroOffset(offset);
((SparkMaxSwerve) sparkMax).updateConfig(cfg);
return true;
} else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
{
SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
cfg.absoluteEncoder.zeroOffset(offset);
((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
return true;
}
offsetFailure.setText("Failure to set Absolute Encoder Offset Error: " + error);
offsetFailure.set(true);
return false;
}

90
swervelib/encoders/TalonSRXEncoderSwerve.java Normal file
View File

@@ -0,0 +1,90 @@
package swervelib.encoders;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import swervelib.motors.SwerveMotor;
import swervelib.motors.TalonSRXSwerve;
/**
* Talon SRX attached absolute encoder.
*/
public class TalonSRXEncoderSwerve extends SwerveAbsoluteEncoder
{
/**
* Multiplying by this converts native Talon SRX units into degrees.
*/
private final double degreesPerSensorUnit;
/**
* Reference to a Talon SRX for polling its attached absolute encoder.
*/
private final WPI_TalonSRX talon;
/**
* Creates a {@link TalonSRXEncoderSwerve}.
*
* @param motor motor to poll the sensor from.
* @param feedbackDevice the feedback device the sensor uses e.g. PWM or Analog.
*/
public TalonSRXEncoderSwerve(SwerveMotor motor, FeedbackDevice feedbackDevice)
{
if (motor instanceof TalonSRXSwerve talonSRXSwerve)
{
talonSRXSwerve.setSelectedFeedbackDevice(feedbackDevice);
this.talon = (WPI_TalonSRX) talonSRXSwerve.getMotor();
// https://v5.docs.ctr-electronics.com/en/stable/ch14_MCSensor.html#sensor-resolution
degreesPerSensorUnit = switch (feedbackDevice)
{
case Analog -> 360.0 / 1024.0;
default -> 360.0 / 4096.0;
};
} else
{
throw new RuntimeException("Motor given to instantiate TalonSRXEncoder is not a WPI_TalonSRX");
}
}
@Override
public void factoryDefault()
{
// Handled in TalonSRXSwerve
}
@Override
public void clearStickyFaults()
{
// Handled in TalonSRXSwerve
}
@Override
public void configure(boolean inverted)
{
talon.setSensorPhase(inverted);
}
@Override
public double getAbsolutePosition()
{
return (talon.getSelectedSensorPosition() * degreesPerSensorUnit) % 360;
}
@Override
public Object getAbsoluteEncoder()
{
return talon;
}
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
talon.setSelectedSensorPosition(talon.getSelectedSensorPosition() + offset / degreesPerSensorUnit);
return true;
}
@Override
public double getVelocity()
{
return talon.getSelectedSensorVelocity() * 10 * degreesPerSensorUnit;
}
}

59
swervelib/imu/NavXSwerve.java
View File

@@ -1,14 +1,12 @@
package swervelib.imu;
import com.kauailabs.navx.frc.AHRS;
import com.studica.frc.AHRS;
import com.studica.frc.AHRS.NavXComType;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.SerialPort;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import java.util.Optional;
import swervelib.telemetry.Alert;
/**
* Communicates with the NavX({@link AHRS}) as the IMU.
@@ -38,9 +36,9 @@ public class NavXSwerve extends SwerveIMU
*
* @param port Serial Port to connect to.
*/
public NavXSwerve(SerialPort.Port port)
public NavXSwerve(NavXComType port)
{
navXError = new Alert("IMU", "Error instantiating NavX.", Alert.AlertType.ERROR_TRACE);
navXError = new Alert("IMU", "Error instantiating NavX.", AlertType.kError);
try
{
/* Communicate w/navX-MXP via the MXP SPI Bus. */
@@ -48,7 +46,6 @@ public class NavXSwerve extends SwerveIMU
/* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
imu = new AHRS(port);
factoryDefault();
SmartDashboard.putData(imu);
} catch (RuntimeException ex)
{
navXError.setText("Error instantiating NavX: " + ex.getMessage());
@@ -56,49 +53,6 @@ public class NavXSwerve extends SwerveIMU
}
}
/**
* Constructor for the NavX({@link AHRS}) swerve.
*
* @param port SPI Port to connect to.
*/
public NavXSwerve(SPI.Port port)
{
try
{
/* Communicate w/navX-MXP via the MXP SPI Bus. */
/* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */
/* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
imu = new AHRS(port);
factoryDefault();
SmartDashboard.putData(imu);
} catch (RuntimeException ex)
{
navXError.setText("Error instantiating NavX: " + ex.getMessage());
navXError.set(true);
}
}
/**
* Constructor for the NavX({@link AHRS}) swerve.
*
* @param port I2C Port to connect to.
*/
public NavXSwerve(I2C.Port port)
{
try
{
/* Communicate w/navX-MXP via the MXP SPI Bus. */
/* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */
/* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
imu = new AHRS(port);
factoryDefault();
SmartDashboard.putData(imu);
} catch (RuntimeException ex)
{
navXError.setText("Error instantiating NavX: " + ex.getMessage());
navXError.set(true);
}
}
/**
* Reset offset to current gyro reading. Does not call NavX({@link AHRS#reset()}) because it has been reported to be
@@ -137,6 +91,7 @@ public class NavXSwerve extends SwerveIMU
public void setInverted(boolean invertIMU)
{
invertedIMU = invertIMU;
setOffset(getRawRotation3d());
}
/**

34
swervelib/imu/Pigeon2Swerve.java
View File

@@ -1,13 +1,17 @@
package swervelib.imu;
import static edu.wpi.first.units.Units.DegreesPerSecond;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.Pigeon2Configuration;
import com.ctre.phoenix6.configs.Pigeon2Configurator;
import com.ctre.phoenix6.hardware.Pigeon2;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.measure.LinearAcceleration;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional;
import java.util.function.Supplier;
/**
* SwerveIMU interface for the {@link Pigeon2}
@@ -36,6 +40,19 @@ public class Pigeon2Swerve extends SwerveIMU
*/
private Pigeon2Configurator cfg;
/**
* X Acceleration supplier
*/
private Supplier<StatusSignal<LinearAcceleration>> xAcc;
/**
* Y Accelleration supplier.
*/
private Supplier<StatusSignal<LinearAcceleration>> yAcc;
/**
* Z Acceleration supplier.
*/
private Supplier<StatusSignal<LinearAcceleration>> zAcc;
/**
* Generate the SwerveIMU for {@link Pigeon2}.
*
@@ -46,6 +63,9 @@ public class Pigeon2Swerve extends SwerveIMU
{
imu = new Pigeon2(canid, canbus);
this.cfg = imu.getConfigurator();
xAcc = imu::getAccelerationX;
yAcc = imu::getAccelerationY;
zAcc = imu::getAccelerationZ;
SmartDashboard.putData(imu);
}
@@ -123,6 +143,7 @@ public class Pigeon2Swerve extends SwerveIMU
return getRawRotation3d().minus(offset);
}
/**
* Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns
* empty.
@@ -132,24 +153,19 @@ public class Pigeon2Swerve extends SwerveIMU
@Override
public Optional<Translation3d> getAccel()
{
// TODO: Switch to suppliers.
StatusSignal<Double> xAcc = imu.getAccelerationX();
StatusSignal<Double> yAcc = imu.getAccelerationY();
StatusSignal<Double> zAcc = imu.getAccelerationZ();
// TODO: Implement later.
return Optional.of(new Translation3d(xAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
yAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
zAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue()).times(9.81 / 16384.0));
return Optional.empty();
}
/**
* Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
*
* @return {@link Double} of the rotation rate as an {@link Optional}.
* @return Rotation rate in DegreesPerSecond.
*/
public double getRate()
{
return imu.getRate();
return imu.getAngularVelocityZWorld().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue().in(DegreesPerSecond);
}
/**

2
swervelib/math/IMULinearMovingAverageFilter.java
View File

@@ -15,7 +15,7 @@ public class IMULinearMovingAverageFilter
/**
* Gain on each reading.
*/
private final double m_inputGain;
private final double m_inputGain;
/**
* Construct a linear moving average fitler

4
swervelib/math/SwerveMath.java
View File

@@ -411,6 +411,10 @@ public class SwerveMath
*/
public static Translation2d scaleTranslation(Translation2d translation, double scalar)
{
if (Math.hypot(translation.getX(), translation.getY()) <= 1.0E-6)
{
return translation;
}
return new Translation2d(translation.getNorm() * scalar, translation.getAngle());
}
}

327
swervelib/motors/SparkFlexSwerve.java
View File

@@ -1,66 +1,90 @@
package swervelib.motors;
import static edu.wpi.first.units.Units.Seconds;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkBase.ControlType;
import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkFlex;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
import com.revrobotics.CANSparkMax;
import com.revrobotics.MotorFeedbackSensor;
import com.revrobotics.REVLibError;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkAnalogSensor;
import com.revrobotics.SparkPIDController;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkFlexConfig;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.Supplier;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
import swervelib.telemetry.Alert;
import swervelib.telemetry.SwerveDriveTelemetry;
/**
* An implementation of {@link CANSparkFlex} as a {@link SwerveMotor}.
* An implementation of {@link SparkFlex} as a {@link SwerveMotor}.
*/
public class SparkFlexSwerve extends SwerveMotor
{
/**
* {@link CANSparkFlex} Instance.
* {@link SparkFlex} Instance.
*/
private final CANSparkFlex motor;
private final SparkFlex motor;
/**
* Integrated encoder.
*/
public RelativeEncoder encoder;
public RelativeEncoder encoder;
/**
* Absolute encoder attached to the SparkMax (if exists)
*/
public SwerveAbsoluteEncoder absoluteEncoder;
public SwerveAbsoluteEncoder absoluteEncoder;
/**
* Closed-loop PID controller.
*/
public SparkPIDController pid;
public SparkClosedLoopController pid;
/**
* Supplier for the velocity of the motor controller.
*/
private Supplier<Double> velocity;
/**
* Supplier for the position of the motor controller.
*/
private Supplier<Double> position;
/**
* Factory default already occurred.
*/
private boolean factoryDefaultOccurred = false;
private boolean factoryDefaultOccurred = false;
/**
* An {@link Alert} for if there is an error configuring the motor.
*/
private Alert failureConfiguring;
private Alert failureConfiguring;
/**
* An {@link Alert} for if the absolute encoder's offset is set in the json instead of the hardware client.
*/
private Alert absoluteEncoderOffsetWarning;
private Alert absoluteEncoderOffsetWarning;
/**
* Configuration object for {@link SparkFlex} motor.
*/
private SparkFlexConfig cfg = new SparkFlexConfig();
/**
* Tracker for changes that need to be pushed.
*/
private boolean cfgUpdated = false;
/**
* Initialize the swerve motor.
*
* @param motor The SwerveMotor as a SparkFlex object.
* @param isDriveMotor Is the motor being initialized a drive motor?
* @param motorType {@link DCMotor} which the {@link SparkFlex} is attached to.
*/
public SparkFlexSwerve(CANSparkFlex motor, boolean isDriveMotor)
public SparkFlexSwerve(SparkFlex motor, boolean isDriveMotor, DCMotor motorType)
{
this.motor = motor;
this.isDriveMotor = isDriveMotor;
@@ -68,32 +92,34 @@ public class SparkFlexSwerve extends SwerveMotor
clearStickyFaults();
encoder = motor.getEncoder();
pid = motor.getPIDController();
pid.setFeedbackDevice(
encoder); // Configure feedback of the PID controller as the integrated encoder.
pid = motor.getClosedLoopController();
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); // Configure feedback of the PID controller as the integrated encoder.
cfgUpdated = true;
// Spin off configurations in a different thread.
// configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback.
failureConfiguring = new Alert("Motors",
"Failure configuring motor " +
motor.getDeviceId(),
Alert.AlertType.WARNING_TRACE);
AlertType.kWarning);
absoluteEncoderOffsetWarning = new Alert("Motors",
"IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the " +
"absoluteEncoderOffset in the Swerve Module JSON!",
Alert.AlertType.WARNING);
AlertType.kWarning);
velocity = encoder::getVelocity;
position = encoder::getPosition;
}
/**
* Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor.
* Initialize the {@link SwerveMotor} as a {@link SparkFlex} connected to a Brushless Motor.
*
* @param id CAN ID of the SparkMax.
* @param isDriveMotor Is the motor being initialized a drive motor?
* @param motorType {@link DCMotor} which the {@link SparkFlex} is attached to.
*/
public SparkFlexSwerve(int id, boolean isDriveMotor)
public SparkFlexSwerve(int id, boolean isDriveMotor, DCMotor motorType)
{
this(new CANSparkFlex(id, MotorType.kBrushless), isDriveMotor);
this(new SparkFlex(id, MotorType.kBrushless), isDriveMotor, motorType);
}
/**
@@ -109,11 +135,33 @@ public class SparkFlexSwerve extends SwerveMotor
{
return;
}
Timer.delay(0.01);
Timer.delay(Units.Milliseconds.of(10).in(Seconds));
}
failureConfiguring.set(true);
}
/**
* Get the current configuration of the {@link SparkFlex}
*
* @return {@link SparkMaxConfig}
*/
public SparkFlexConfig getConfig()
{
return cfg;
}
/**
* Update the config for the {@link SparkFlex}
*
* @param cfgGiven Given {@link SparkFlexConfig} which should have minimal modifications.
*/
public void updateConfig(SparkFlexConfig cfgGiven)
{
cfg.apply(cfgGiven);
configureSparkFlex(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
cfgUpdated = false;
}
/**
* Set the voltage compensation for the swerve module motor.
*
@@ -122,7 +170,8 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void setVoltageCompensation(double nominalVoltage)
{
configureSparkFlex(() -> motor.enableVoltageCompensation(nominalVoltage));
cfg.voltageCompensation(nominalVoltage);
cfgUpdated = true;
}
/**
@@ -134,7 +183,9 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void setCurrentLimit(int currentLimit)
{
configureSparkFlex(() -> motor.setSmartCurrentLimit(currentLimit));
cfg.smartCurrentLimit(currentLimit);
cfgUpdated = true;
}
/**
@@ -145,8 +196,9 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void setLoopRampRate(double rampRate)
{
configureSparkFlex(() -> motor.setOpenLoopRampRate(rampRate));
configureSparkFlex(() -> motor.setClosedLoopRampRate(rampRate));
cfg.closedLoopRampRate(rampRate)
.openLoopRampRate(rampRate);
cfgUpdated = true;
}
/**
@@ -160,6 +212,21 @@ public class SparkFlexSwerve extends SwerveMotor
return motor;
}
/**
* Get the {@link DCMotor} of the motor class.
*
* @return {@link DCMotor} of this type.
*/
@Override
public DCMotor getSimMotor()
{
if (simMotor == null)
{
simMotor = DCMotor.getNeoVortex(1);
}
return simMotor;
}
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*
@@ -177,11 +244,7 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void factoryDefaults()
{
if (!factoryDefaultOccurred)
{
configureSparkFlex(motor::restoreFactoryDefaults);
factoryDefaultOccurred = true;
}
// Do nothing
}
/**
@@ -205,12 +268,20 @@ public class SparkFlexSwerve extends SwerveMotor
if (encoder == null)
{
absoluteEncoder = null;
configureSparkFlex(() -> pid.setFeedbackDevice(this.encoder));
} else if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor)
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
cfgUpdated = true;
velocity = this.encoder::getVelocity;
position = this.encoder::getPosition;
} else if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
cfgUpdated = true;
absoluteEncoderOffsetWarning.set(true);
absoluteEncoder = encoder;
configureSparkFlex(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder()));
velocity = absoluteEncoder::getVelocity;
position = absoluteEncoder::getAbsolutePosition;
}
return this;
}
@@ -223,39 +294,70 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void configureIntegratedEncoder(double positionConversionFactor)
{
cfg.signals
.absoluteEncoderPositionAlwaysOn(false)
.absoluteEncoderVelocityAlwaysOn(false)
.analogPositionAlwaysOn(false)
.analogVelocityAlwaysOn(false)
.analogVoltageAlwaysOn(false)
.externalOrAltEncoderPositionAlwaysOn(false)
.externalOrAltEncoderVelocityAlwaysOn(false)
.primaryEncoderPositionAlwaysOn(false)
.primaryEncoderVelocityAlwaysOn(false)
.iAccumulationAlwaysOn(false)
.appliedOutputPeriodMs(10)
.faultsPeriodMs(20)
;
if (absoluteEncoder == null)
{
configureSparkFlex(() -> encoder.setPositionConversionFactor(positionConversionFactor));
configureSparkFlex(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60));
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
cfg.encoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
// Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
// Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement)
// Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
// https://www.chiefdelphi.com/t/shooter-encoder/400211/11
// This value was taken from:
// https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
// and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
cfg.encoder
.quadratureMeasurementPeriod(10)
.quadratureAverageDepth(2);
// Taken from
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
configureCANStatusFrames(10, 20, 20, 500, 500);
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
// Unused frames can be set to 65535 to decrease CAN ultilization.
cfg.signals
.primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
.primaryEncoderPositionAlwaysOn(true)
.primaryEncoderPositionPeriodMs(20);
} else
{
configureSparkFlex(() -> {
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
positionConversionFactor);
} else
{
return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
positionConversionFactor);
}
});
configureSparkFlex(() -> {
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
positionConversionFactor / 60);
} else
{
return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
positionConversionFactor / 60);
}
});
// By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5
// This needs to be set to 20ms or under to properly update the swerve module position for odometry
// Configuration taken from 3005, the team who helped develop the Max Swerve:
// https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244
// Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max.
// From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill,
// with limited testing 19ms did not return the same value while the module was constatntly rotating.
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
cfg.signals
.absoluteEncoderPositionAlwaysOn(true)
.absoluteEncoderPositionPeriodMs(20);
cfg.absoluteEncoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
}
}
cfgUpdated = true;
}
/**
@@ -266,15 +368,10 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void configurePIDF(PIDFConfig config)
{
// int pidSlot =
// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
int pidSlot = 0;
configureSparkFlex(() -> pid.setP(config.p, pidSlot));
configureSparkFlex(() -> pid.setI(config.i, pidSlot));
configureSparkFlex(() -> pid.setD(config.d, pidSlot));
configureSparkFlex(() -> pid.setFF(config.f, pidSlot));
configureSparkFlex(() -> pid.setIZone(config.iz, pidSlot));
configureSparkFlex(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot));
cfg.closedLoop.pidf(config.p, config.i, config.d, config.f)
.iZone(config.iz)
.outputRange(config.output.min, config.output.max);
cfgUpdated = true;
}
/**
@@ -286,30 +383,11 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void configurePIDWrapping(double minInput, double maxInput)
{
configureSparkFlex(() -> pid.setPositionPIDWrappingEnabled(true));
configureSparkFlex(() -> pid.setPositionPIDWrappingMinInput(minInput));
configureSparkFlex(() -> pid.setPositionPIDWrappingMaxInput(maxInput));
}
cfg.closedLoop
.positionWrappingEnabled(true)
.positionWrappingInputRange(minInput, maxInput);
cfgUpdated = true;
/**
* Set the CAN status frames.
*
* @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower
* @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current
* @param CANStatus2 Motor Position
* @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position
* @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position
*/
public void configureCANStatusFrames(
int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4)
{
configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0));
configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1));
configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2));
configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3));
configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4));
// TODO: Configure Status Frame 5 and 6 if necessary
// https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
}
/**
@@ -320,7 +398,8 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void setMotorBrake(boolean isBrakeMode)
{
configureSparkFlex(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast));
cfg.idleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast);
cfgUpdated = true;
}
/**
@@ -331,10 +410,8 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void setInverted(boolean inverted)
{
configureSparkFlex(() -> {
motor.setInverted(inverted);
return motor.getLastError();
});
cfg.inverted(inverted);
cfgUpdated = true;
}
/**
@@ -343,13 +420,8 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void burnFlash()
{
try
{
Thread.sleep(200);
} catch (Exception e)
{
}
configureSparkFlex(() -> motor.burnFlash());
motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
cfgUpdated = false;
}
/**
@@ -372,11 +444,14 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void setReference(double setpoint, double feedforward)
{
boolean possibleBurnOutIssue = true;
// int pidSlot =
// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
int pidSlot = 0;
if (cfgUpdated)
{
burnFlash();
Timer.delay(0.1); // Give 100ms to apply changes
}
if (isDriveMotor)
{
configureSparkFlex(() ->
@@ -454,7 +529,7 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public double getVelocity()
{
return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity();
return velocity.get();
}
/**
@@ -465,7 +540,7 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public double getPosition()
{
return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getAbsolutePosition();
return position.get();
}
/**
@@ -482,22 +557,4 @@ public class SparkFlexSwerve extends SwerveMotor
}
}
/**
* REV Slots for PID configuration.
*/
enum SparkMAX_slotIdx
{
/**
* Slot 1, used for position PID's.
*/
Position,
/**
* Slot 2, used for velocity PID's.
*/
Velocity,
/**
* Slot 3, used arbitrarily. (Documentation show simulations).
*/
Simulation
}
}

395
swervelib/motors/SparkMaxBrushedMotorSwerve.java
View File

@@ -1,24 +1,35 @@
package swervelib.motors;
import static edu.wpi.first.units.Units.Seconds;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkBase.ControlType;
import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
import com.revrobotics.CANSparkMax;
import com.revrobotics.REVLibError;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkMaxAlternateEncoder;
import com.revrobotics.SparkPIDController;
import com.revrobotics.SparkRelativeEncoder.Type;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import java.util.Optional;
import java.util.function.Supplier;
import swervelib.encoders.SparkMaxAnalogEncoderSwerve;
import swervelib.encoders.SparkMaxEncoderSwerve;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
import swervelib.telemetry.Alert;
import swervelib.telemetry.SwerveDriveTelemetry;
/**
* Brushed motor control with {@link CANSparkMax}.
* Brushed motor control with {@link SparkMax}.
*/
public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{
@@ -26,59 +37,74 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
/**
* SparkMAX Instance.
*/
private final CANSparkMax motor;
private final SparkMax motor;
/**
* Absolute encoder attached to the SparkMax (if exists)
*/
public AbsoluteEncoder absoluteEncoder;
public SwerveAbsoluteEncoder absoluteEncoder;
/**
* Integrated encoder.
*/
public RelativeEncoder encoder;
public Optional<RelativeEncoder> encoder = Optional.empty();
/**
* Closed-loop PID controller.
*/
public SparkPIDController pid;
public SparkClosedLoopController pid;
/**
* Supplier for the velocity of the motor controller.
*/
private Supplier<Double> velocity;
/**
* Supplier for the position of the motor controller.
*/
private Supplier<Double> position;
/**
* Factory default already occurred.
*/
private boolean factoryDefaultOccurred = false;
private boolean factoryDefaultOccurred = false;
/**
* An {@link Alert} for if the motor has no encoder.
*/
private Alert noEncoderAlert;
private Alert noEncoderAlert;
/**
* An {@link Alert} for if there is an error configuring the motor.
*/
private Alert failureConfiguringAlert;
private Alert failureConfiguringAlert;
/**
* An {@link Alert} for if the motor has no encoder defined.
*/
private Alert noEncoderDefinedAlert;
private Alert noEncoderDefinedAlert;
/**
* Configuration object for {@link SparkMax} motor.
*/
private SparkMaxConfig cfg = new SparkMaxConfig();
/**
* Tracker for changes that need to be pushed.
*/
private boolean cfgUpdated = false;
/**
* Initialize the swerve motor.
*
* @param motor The SwerveMotor as a SparkMax object.
* @param isDriveMotor Is the motor being initialized a drive motor?
* @param encoderType {@link Type} of encoder to use for the {@link CANSparkMax} device.
* @param encoderType {@link Type} of encoder to use for the {@link SparkMax} device.
* @param countsPerRevolution The number of encoder pulses for the {@link Type} encoder per revolution.
* @param useDataPortQuadEncoder Use the encoder attached to the data port of the spark max for a quadrature encoder.
* @param motorType {@link DCMotor} which the {@link SparkMax} is attached to.
*/
public SparkMaxBrushedMotorSwerve(CANSparkMax motor, boolean isDriveMotor, Type encoderType, int countsPerRevolution,
boolean useDataPortQuadEncoder)
public SparkMaxBrushedMotorSwerve(SparkMax motor, boolean isDriveMotor, Type encoderType, int countsPerRevolution,
boolean useDataPortQuadEncoder, DCMotor motorType)
{
noEncoderAlert = new Alert("Motors",
"Cannot use motor without encoder.",
Alert.AlertType.ERROR_TRACE);
AlertType.kError);
failureConfiguringAlert = new Alert("Motors",
"Failure configuring motor " + motor.getDeviceId(),
Alert.AlertType.WARNING_TRACE);
AlertType.kWarning);
noEncoderDefinedAlert = new Alert("Motors",
"An encoder MUST be defined to work with a SparkMAX",
Alert.AlertType.ERROR_TRACE);
AlertType.kError);
// Drive motors **MUST** have an encoder attached.
if (isDriveMotor && encoderType == Type.kNoSensor)
@@ -95,42 +121,61 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
this.motor = motor;
this.isDriveMotor = isDriveMotor;
this.simMotor = motorType;
factoryDefaults();
clearStickyFaults();
// Get the onboard PID controller.
pid = motor.getPIDController();
pid = motor.getClosedLoopController();
// If there is a sensor attached to the data port or encoder port set the relative encoder.
if (isDriveMotor || (encoderType != Type.kNoSensor || useDataPortQuadEncoder))
{
this.encoder = useDataPortQuadEncoder ?
motor.getAlternateEncoder(SparkMaxAlternateEncoder.Type.kQuadrature, countsPerRevolution) :
motor.getEncoder(encoderType, countsPerRevolution);
// Configure feedback of the PID controller as the integrated encoder.
configureSparkMax(() -> pid.setFeedbackDevice(encoder));
if (useDataPortQuadEncoder)
{
this.encoder = Optional.of(motor.getAlternateEncoder());
cfg.alternateEncoder.countsPerRevolution(countsPerRevolution);
// Configure feedback of the PID controller as the integrated encoder.
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAlternateOrExternalEncoder);
} else
{
this.encoder = Optional.of(motor.getEncoder());
cfg.encoder.countsPerRevolution(countsPerRevolution);
// Configure feedback of the PID controller as the integrated encoder.
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
}
cfgUpdated = true;
}
encoder.ifPresentOrElse((RelativeEncoder enc) -> {
velocity = enc::getVelocity;
position = enc::getPosition;
}, () -> {
noEncoderDefinedAlert.set(true);
});
// Spin off configurations in a different thread.
// configureSparkMax(() -> motor.setCANTimeout(0)); // Commented it out because it prevents feedback.
}
/**
* Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor.
* Initialize the {@link SwerveMotor} as a {@link SparkMax} connected to a Brushless Motor.
*
* @param id CAN ID of the SparkMax.
* @param isDriveMotor Is the motor being initialized a drive motor?
* @param encoderType {@link Type} of encoder to use for the {@link CANSparkMax} device.
* @param encoderType {@link Type} of encoder to use for the {@link SparkMax} device.
* @param countsPerRevolution The number of encoder pulses for the {@link Type} encoder per revolution.
* @param useDataPortQuadEncoder Use the encoder attached to the data port of the spark max for a quadrature encoder.
* @param motorType Motor type controlled by the {@link SparkMax} motor controller.
*/
public SparkMaxBrushedMotorSwerve(int id, boolean isDriveMotor, Type encoderType, int countsPerRevolution,
boolean useDataPortQuadEncoder)
boolean useDataPortQuadEncoder, DCMotor motorType)
{
this(new CANSparkMax(id, MotorType.kBrushed), isDriveMotor, encoderType, countsPerRevolution,
useDataPortQuadEncoder);
this(new SparkMax(id, MotorType.kBrushed), isDriveMotor, encoderType, countsPerRevolution,
useDataPortQuadEncoder, motorType);
}
/**
@@ -146,11 +191,33 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{
return;
}
Timer.delay(0.01);
Timer.delay(Units.Milliseconds.of(10).in(Seconds));
}
failureConfiguringAlert.set(true);
}
/**
* Get the current configuration of the {@link SparkMax}
*
* @return {@link SparkMaxConfig}
*/
public SparkMaxConfig getConfig()
{
return cfg;
}
/**
* Update the config for the {@link SparkMax}
*
* @param cfgGiven Given {@link SparkMaxConfig} which should have minimal modifications.
*/
public void updateConfig(SparkMaxConfig cfgGiven)
{
cfg.apply(cfgGiven);
configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
cfgUpdated = false;
}
/**
* Set the voltage compensation for the swerve module motor.
*
@@ -159,7 +226,8 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void setVoltageCompensation(double nominalVoltage)
{
configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage));
cfg.voltageCompensation(nominalVoltage);
cfgUpdated = true;
}
/**
@@ -171,7 +239,8 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void setCurrentLimit(int currentLimit)
{
configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit));
cfg.smartCurrentLimit(currentLimit);
cfgUpdated = true;
}
/**
@@ -182,8 +251,9 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void setLoopRampRate(double rampRate)
{
configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate));
configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate));
cfg.closedLoopRampRate(rampRate)
.openLoopRampRate(rampRate);
cfgUpdated = true;
}
/**
@@ -197,6 +267,21 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
return motor;
}
/**
* Get the {@link DCMotor} of the motor class.
*
* @return {@link DCMotor} of this type.
*/
@Override
public DCMotor getSimMotor()
{
if (simMotor == null)
{
simMotor = DCMotor.getCIM(1);
}
return simMotor;
}
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*
@@ -214,11 +299,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void factoryDefaults()
{
if (!factoryDefaultOccurred)
{
configureSparkMax(motor::restoreFactoryDefaults);
factoryDefaultOccurred = true;
}
// Do nothing
}
/**
@@ -242,13 +323,32 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
if (encoder == null)
{
absoluteEncoder = null;
configureSparkMax(() -> pid.setFeedbackDevice(this.encoder));
} else if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
cfgUpdated = true;
this.encoder.ifPresentOrElse((RelativeEncoder enc) -> {
velocity = enc::getVelocity;
position = enc::getPosition;
}, () -> {
noEncoderDefinedAlert.set(true);
});
burnFlash();
} else if (encoder instanceof SparkMaxAnalogEncoderSwerve || encoder instanceof SparkMaxEncoderSwerve)
{
absoluteEncoder = (AbsoluteEncoder) encoder.getAbsoluteEncoder();
configureSparkMax(() -> pid.setFeedbackDevice(absoluteEncoder));
cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve
? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder);
cfgUpdated = true;
DriverStation.reportWarning(
"IF possible configure the encoder offset in the REV Hardware Client instead of using the" +
" absoluteEncoderOffset in the Swerve Module JSON!",
false);
absoluteEncoder = encoder;
velocity = absoluteEncoder::getVelocity;
position = absoluteEncoder::getAbsolutePosition;
noEncoderDefinedAlert.set(false);
}
if (absoluteEncoder == null && this.encoder == null)
if (absoluteEncoder == null && this.encoder.isEmpty())
{
noEncoderDefinedAlert.set(true);
throw new RuntimeException("An encoder MUST be defined to work with a SparkMAX");
@@ -264,19 +364,79 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void configureIntegratedEncoder(double positionConversionFactor)
{
cfg.signals
.absoluteEncoderPositionAlwaysOn(false)
.absoluteEncoderVelocityAlwaysOn(false)
.analogPositionAlwaysOn(false)
.analogVelocityAlwaysOn(false)
.analogVoltageAlwaysOn(false)
.externalOrAltEncoderPositionAlwaysOn(false)
.externalOrAltEncoderVelocityAlwaysOn(false)
.primaryEncoderPositionAlwaysOn(false)
.primaryEncoderVelocityAlwaysOn(false)
.iAccumulationAlwaysOn(false)
.appliedOutputPeriodMs(10)
.faultsPeriodMs(20);
if (absoluteEncoder == null)
{
configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor));
configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60));
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
cfg.encoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
// Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
// Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement)
// Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
// https://www.chiefdelphi.com/t/shooter-encoder/400211/11
// This value was taken from:
// https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
// and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
cfg.encoder
.quadratureMeasurementPeriod(10)
.quadratureAverageDepth(2);
// Taken from
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
configureCANStatusFrames(10, 20, 20, 500, 500);
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
// Unused frames can be set to 65535 to decrease CAN ultilization.
cfg.signals
.primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
.primaryEncoderPositionAlwaysOn(true)
.primaryEncoderPositionPeriodMs(20);
} else
{
configureSparkMax(() -> absoluteEncoder.setPositionConversionFactor(positionConversionFactor));
configureSparkMax(() -> absoluteEncoder.setVelocityConversionFactor(positionConversionFactor / 60));
// By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5
// This needs to be set to 20ms or under to properly update the swerve module position for odometry
// Configuration taken from 3005, the team who helped develop the Max Swerve:
// https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244
// Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max.
// From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill,
// with limited testing 19ms did not return the same value while the module was constatntly rotating.
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
cfg.signals
.absoluteEncoderPositionAlwaysOn(true)
.absoluteEncoderPositionPeriodMs(20);
cfg.absoluteEncoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
} else
{
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAnalogSensor);
cfg.signals
.analogVoltageAlwaysOn(true)
.analogPositionAlwaysOn(true)
.analogVoltagePeriodMs(20)
.analogPositionPeriodMs(20);
cfg.analogSensor
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
}
}
cfgUpdated = true;
}
/**
@@ -287,16 +447,10 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void configurePIDF(PIDFConfig config)
{
// int pidSlot =
// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
int pidSlot = 0;
configureSparkMax(() -> pid.setP(config.p, pidSlot));
configureSparkMax(() -> pid.setI(config.i, pidSlot));
configureSparkMax(() -> pid.setD(config.d, pidSlot));
configureSparkMax(() -> pid.setFF(config.f, pidSlot));
configureSparkMax(() -> pid.setIZone(config.iz, pidSlot));
configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot));
cfg.closedLoop.pidf(config.p, config.i, config.d, config.f)
.iZone(config.iz)
.outputRange(config.output.min, config.output.max);
cfgUpdated = true;
}
/**
@@ -308,30 +462,11 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void configurePIDWrapping(double minInput, double maxInput)
{
configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true));
configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput));
configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(maxInput));
}
cfg.closedLoop
.positionWrappingEnabled(true)
.positionWrappingInputRange(minInput, maxInput);
cfgUpdated = true;
/**
* Set the CAN status frames.
*
* @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower
* @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current
* @param CANStatus2 Motor Position
* @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position
* @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position
*/
public void configureCANStatusFrames(
int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4)
{
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4));
// TODO: Configure Status Frame 5 and 6 if necessary
// https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
}
/**
@@ -342,7 +477,8 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void setMotorBrake(boolean isBrakeMode)
{
configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast));
cfg.idleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast);
cfgUpdated = true;
}
/**
@@ -353,10 +489,8 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void setInverted(boolean inverted)
{
configureSparkMax(() -> {
motor.setInverted(inverted);
return motor.getLastError();
});
cfg.inverted(inverted);
cfgUpdated = true;
}
/**
@@ -365,7 +499,8 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void burnFlash()
{
configureSparkMax(() -> motor.burnFlash());
motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
cfgUpdated = false;
}
/**
@@ -388,16 +523,37 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void setReference(double setpoint, double feedforward)
{
// int pidSlot =
// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
int pidSlot = 0;
configureSparkMax(() ->
pid.setReference(
setpoint,
isDriveMotor ? ControlType.kVelocity : ControlType.kPosition,
pidSlot,
feedforward)
);
if (cfgUpdated)
{
burnFlash();
Timer.delay(0.1); // Give 100ms to apply changes
}
if (isDriveMotor)
{
configureSparkMax(() ->
pid.setReference(
setpoint,
ControlType.kVelocity,
pidSlot,
feedforward));
} else
{
configureSparkMax(() ->
pid.setReference(
setpoint,
ControlType.kPosition,
pidSlot,
feedforward));
if (SwerveDriveTelemetry.isSimulation)
{
encoder.ifPresent((RelativeEncoder enc) -> {
enc.setPosition(setpoint);
});
}
}
}
/**
@@ -454,7 +610,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public double getVelocity()
{
return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity();
return velocity.get();
}
/**
@@ -465,7 +621,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public double getPosition()
{
return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getPosition();
return position.get();
}
/**
@@ -478,7 +634,28 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{
if (absoluteEncoder == null)
{
configureSparkMax(() -> encoder.setPosition(position));
encoder.ifPresent((RelativeEncoder enc) -> {
configureSparkMax(() -> enc.setPosition(position));
});
}
}
/**
* Type for encoder for {@link SparkMax}
*/
public enum Type
{
/**
* NO sensor
*/
kNoSensor,
/**
* Hall sensor attached to dataport
*/
kHallSensor,
/**
* Quad encoder attached to alt
*/
kQuadrature,
}
}

313
swervelib/motors/SparkMaxSwerve.java
View File

@@ -1,91 +1,112 @@
package swervelib.motors;
import static edu.wpi.first.units.Units.Seconds;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkBase.ControlType;
import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
import com.revrobotics.CANSparkMax;
import com.revrobotics.MotorFeedbackSensor;
import com.revrobotics.REVLibError;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkAnalogSensor;
import com.revrobotics.SparkPIDController;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.Supplier;
import swervelib.encoders.SparkMaxAnalogEncoderSwerve;
import swervelib.encoders.SparkMaxEncoderSwerve;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
import swervelib.telemetry.SwerveDriveTelemetry;
/**
* An implementation of {@link CANSparkMax} as a {@link SwerveMotor}.
* An implementation of {@link com.revrobotics.spark.SparkMax} as a {@link SwerveMotor}.
*/
public class SparkMaxSwerve extends SwerveMotor
{
/**
* {@link CANSparkMax} Instance.
* {@link SparkMax} Instance.
*/
private final CANSparkMax motor;
private final SparkMax motor;
/**
* Integrated encoder.
*/
public RelativeEncoder encoder;
public RelativeEncoder encoder;
/**
* Absolute encoder attached to the SparkMax (if exists)
*/
public SwerveAbsoluteEncoder absoluteEncoder;
public SwerveAbsoluteEncoder absoluteEncoder;
/**
* Closed-loop PID controller.
*/
public SparkPIDController pid;
public SparkClosedLoopController pid;
/**
* Factory default already occurred.
*/
private boolean factoryDefaultOccurred = false;
private boolean factoryDefaultOccurred = false;
/**
* Supplier for the velocity of the motor controller.
*/
private Supplier<Double> velocity;
private Supplier<Double> velocity;
/**
* Supplier for the position of the motor controller.
*/
private Supplier<Double> position;
private Supplier<Double> position;
/**
* Configuration object for {@link SparkMax} motor.
*/
private SparkMaxConfig cfg = new SparkMaxConfig();
/**
* Tracker for changes that need to be pushed.
*/
private boolean cfgUpdated = false;
/**
* Initialize the swerve motor.
*
* @param motor The SwerveMotor as a SparkMax object.
* @param isDriveMotor Is the motor being initialized a drive motor?
* @param motorType Motor type controlled by the {@link SparkMax} motor controller.
*/
public SparkMaxSwerve(CANSparkMax motor, boolean isDriveMotor)
public SparkMaxSwerve(SparkMax motor, boolean isDriveMotor, DCMotor motorType)
{
this.motor = motor;
this.isDriveMotor = isDriveMotor;
this.simMotor = motorType;
factoryDefaults();
clearStickyFaults();
encoder = motor.getEncoder();
pid = motor.getPIDController();
pid.setFeedbackDevice(
encoder); // Configure feedback of the PID controller as the integrated encoder.
pid = motor.getClosedLoopController();
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); // Configure feedback of the PID controller as the integrated encoder.
cfgUpdated = true;
velocity = encoder::getVelocity;
position = encoder::getPosition;
// Spin off configurations in a different thread.
// configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback.
}
/**
* Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor.
* Initialize the {@link SwerveMotor} as a {@link SparkMax} connected to a Brushless Motor.
*
* @param id CAN ID of the SparkMax.
* @param isDriveMotor Is the motor being initialized a drive motor?
* @param motorType Motor type controlled by the {@link SparkMax} motor controller.
*/
public SparkMaxSwerve(int id, boolean isDriveMotor)
public SparkMaxSwerve(int id, boolean isDriveMotor, DCMotor motorType)
{
this(new CANSparkMax(id, MotorType.kBrushless), isDriveMotor);
this(new SparkMax(id, MotorType.kBrushless), isDriveMotor, motorType);
}
/**
@@ -101,11 +122,33 @@ public class SparkMaxSwerve extends SwerveMotor
{
return;
}
Timer.delay(0.01);
Timer.delay(Units.Milliseconds.of(10).in(Seconds));
}
DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true);
}
/**
* Get the current configuration of the {@link SparkMax}
*
* @return {@link SparkMaxConfig}
*/
public SparkMaxConfig getConfig()
{
return cfg;
}
/**
* Update the config for the {@link SparkMax}
*
* @param cfgGiven Given {@link SparkMaxConfig} which should have minimal modifications.
*/
public void updateConfig(SparkMaxConfig cfgGiven)
{
cfg.apply(cfgGiven);
configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
cfgUpdated = false;
}
/**
* Set the voltage compensation for the swerve module motor.
*
@@ -114,7 +157,8 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setVoltageCompensation(double nominalVoltage)
{
configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage));
cfg.voltageCompensation(nominalVoltage);
cfgUpdated = true;
}
/**
@@ -126,7 +170,9 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setCurrentLimit(int currentLimit)
{
configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit));
cfg.smartCurrentLimit(currentLimit);
cfgUpdated = true;
}
/**
@@ -137,8 +183,10 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setLoopRampRate(double rampRate)
{
configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate));
configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate));
cfg.closedLoopRampRate(rampRate)
.openLoopRampRate(rampRate);
cfgUpdated = true;
}
/**
@@ -152,6 +200,21 @@ public class SparkMaxSwerve extends SwerveMotor
return motor;
}
/**
* Get the {@link DCMotor} of the motor class.
*
* @return {@link DCMotor} of this type.
*/
@Override
public DCMotor getSimMotor()
{
if (simMotor == null)
{
simMotor = DCMotor.getNEO(1);
}
return simMotor;
}
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*
@@ -169,11 +232,7 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void factoryDefaults()
{
if (!factoryDefaultOccurred)
{
configureSparkMax(motor::restoreFactoryDefaults);
factoryDefaultOccurred = true;
}
// Do nothing
}
/**
@@ -197,17 +256,23 @@ public class SparkMaxSwerve extends SwerveMotor
if (encoder == null)
{
absoluteEncoder = null;
configureSparkMax(() -> pid.setFeedbackDevice(this.encoder));
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
cfgUpdated = true;
velocity = this.encoder::getVelocity;
position = this.encoder::getPosition;
} else if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor)
burnFlash();
} else if (encoder instanceof SparkMaxAnalogEncoderSwerve || encoder instanceof SparkMaxEncoderSwerve)
{
cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve
? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder);
cfgUpdated = true;
DriverStation.reportWarning(
"IF possible configure the encoder offset in the REV Hardware Client instead of using the" +
" absoluteEncoderOffset in the Swerve Module JSON!",
false);
absoluteEncoder = encoder;
configureSparkMax(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder()));
velocity = absoluteEncoder::getVelocity;
position = absoluteEncoder::getAbsolutePosition;
}
@@ -222,10 +287,26 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void configureIntegratedEncoder(double positionConversionFactor)
{
cfg.signals
.absoluteEncoderPositionAlwaysOn(false)
.absoluteEncoderVelocityAlwaysOn(false)
.analogPositionAlwaysOn(false)
.analogVelocityAlwaysOn(false)
.analogVoltageAlwaysOn(false)
.externalOrAltEncoderPositionAlwaysOn(false)
.externalOrAltEncoderVelocityAlwaysOn(false)
.primaryEncoderPositionAlwaysOn(false)
.primaryEncoderVelocityAlwaysOn(false)
.iAccumulationAlwaysOn(false)
.appliedOutputPeriodMs(10)
.faultsPeriodMs(20);
if (absoluteEncoder == null)
{
configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor));
configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60));
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
cfg.encoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
// Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
// Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement)
// Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
@@ -233,13 +314,18 @@ public class SparkMaxSwerve extends SwerveMotor
// This value was taken from:
// https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
// and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
configureSparkMax(() -> encoder.setMeasurementPeriod(10));
configureSparkMax(() -> encoder.setAverageDepth(2));
cfg.encoder
.quadratureMeasurementPeriod(10)
.quadratureAverageDepth(2);
// Taken from
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
// Unused frames can be set to 65535 to decrease CAN ultilization.
configureCANStatusFrames(10, 20, 20, 500, 500, 200, 200);
cfg.signals
.primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
.primaryEncoderPositionAlwaysOn(true)
.primaryEncoderPositionPeriodMs(20);
} else
{
// By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5
@@ -251,36 +337,32 @@ public class SparkMaxSwerve extends SwerveMotor
// with limited testing 19ms did not return the same value while the module was constatntly rotating.
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
configureCANStatusFrames(100, 20, 20, 200, 20, 8, 50);
}
// Need to test on analog encoders but the same concept should apply for them, worst thing that could happen is slightly more can use
else if (absoluteEncoder.getAbsoluteEncoder() instanceof SparkAnalogSensor)
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
cfg.signals
.absoluteEncoderPositionAlwaysOn(true)
.absoluteEncoderPositionPeriodMs(20);
cfg.absoluteEncoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
} else
{
configureCANStatusFrames(100, 20, 20, 19, 200, 200, 200);
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAnalogSensor);
cfg.signals
.analogVoltageAlwaysOn(true)
.analogPositionAlwaysOn(true)
.analogVoltagePeriodMs(20)
.analogPositionPeriodMs(20);
cfg.analogSensor
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
}
configureSparkMax(() -> {
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
positionConversionFactor);
} else
{
return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
positionConversionFactor);
}
});
configureSparkMax(() -> {
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
positionConversionFactor / 60);
} else
{
return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
positionConversionFactor / 60);
}
});
}
cfgUpdated = true;
}
/**
@@ -291,15 +373,11 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void configurePIDF(PIDFConfig config)
{
// int pidSlot =
// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
int pidSlot = 0;
configureSparkMax(() -> pid.setP(config.p));
configureSparkMax(() -> pid.setI(config.i));
configureSparkMax(() -> pid.setD(config.d));
configureSparkMax(() -> pid.setFF(config.f));
configureSparkMax(() -> pid.setIZone(config.iz));
configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max));
cfg.closedLoop.pidf(config.p, config.i, config.d, config.f)
.iZone(config.iz)
.outputRange(config.output.min, config.output.max);
cfgUpdated = true;
}
/**
@@ -311,33 +389,11 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void configurePIDWrapping(double minInput, double maxInput)
{
configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true));
configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput));
configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(maxInput));
}
cfg.closedLoop
.positionWrappingEnabled(true)
.positionWrappingInputRange(minInput, maxInput);
cfgUpdated = true;
/**
* Set the CAN status frames.
*
* @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower
* @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current
* @param CANStatus2 Motor Position
* @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position
* @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position
* @param CANStatus5 Duty Cycle Absolute Encoder Position, Duty Cycle Absolute Encoder Absolute Angle
* @param CANStatus6 Duty Cycle Absolute Encoder Velocity, Duty Cycle Absolute Encoder Frequency
*/
public void configureCANStatusFrames(
int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4, int CANStatus5, int CANStatus6)
{
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus5, CANStatus5));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus6, CANStatus6));
// https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
}
/**
@@ -348,7 +404,9 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setMotorBrake(boolean isBrakeMode)
{
configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast));
cfg.idleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast);
cfgUpdated = true;
}
/**
@@ -359,10 +417,8 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setInverted(boolean inverted)
{
configureSparkMax(() -> {
motor.setInverted(inverted);
return motor.getLastError();
});
cfg.inverted(inverted);
cfgUpdated = true;
}
/**
@@ -371,13 +427,8 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void burnFlash()
{
try
{
Thread.sleep(200);
} catch (Exception e)
{
}
configureSparkMax(() -> motor.burnFlash());
motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
cfgUpdated = false;
}
/**
@@ -400,11 +451,14 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setReference(double setpoint, double feedforward)
{
boolean possibleBurnOutIssue = true;
// int pidSlot =
// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
int pidSlot = 0;
if (cfgUpdated)
{
burnFlash();
Timer.delay(0.1); // Give 100ms to apply changes
}
if (isDriveMotor)
{
configureSparkMax(() ->
@@ -509,23 +563,4 @@ public class SparkMaxSwerve extends SwerveMotor
configureSparkMax(() -> encoder.setPosition(position));
}
}
/**
* REV Slots for PID configuration.
*/
enum SparkMAX_slotIdx
{
/**
* Slot 1, used for position PID's.
*/
Position,
/**
* Slot 2, used for velocity PID's.
*/
Velocity,
/**
* Slot 3, used arbitrarily. (Documentation show simulations).
*/
Simulation
}
}

14
swervelib/motors/SwerveMotor.java
View File

@@ -1,5 +1,6 @@
package swervelib.motors;
import edu.wpi.first.math.system.plant.DCMotor;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
@@ -13,6 +14,12 @@ public abstract class SwerveMotor
* The maximum amount of times the swerve motor will attempt to configure a motor if failures occur.
*/
public final int maximumRetries = 5;
/**
* Sim motor to use, defaulted in {@link SwerveMotor#getSimMotor()}, but can be overridden here. <br/> NOTE: This will
* not change the simulation motor type! It is intended for use only if you are utilizing Feedforwards from
* PathPlanner.
*/
public DCMotor simMotor;
/**
* Whether the swerve motor is a drive motor.
*/
@@ -172,6 +179,13 @@ public abstract class SwerveMotor
*/
public abstract Object getMotor();
/**
* Get the {@link DCMotor} of the motor class.
*
* @return {@link DCMotor} of this type.
*/
public abstract DCMotor getSimMotor();
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*

129
swervelib/motors/TalonFXSwerve.java
View File

@@ -1,12 +1,18 @@
package swervelib.motors;
import static edu.wpi.first.units.Units.Degrees;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.Volts;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfigurator;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.system.plant.DCMotor;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
import swervelib.telemetry.SwerveDriveTelemetry;
@@ -20,39 +26,39 @@ public class TalonFXSwerve extends SwerveMotor
/**
* Wait time for status frames to show up.
*/
public static double STATUS_TIMEOUT_SECONDS = 0.02;
public static double STATUS_TIMEOUT_SECONDS = 0.02;
/**
* Factory default already occurred.
*/
private final boolean factoryDefaultOccurred = false;
private final boolean factoryDefaultOccurred = false;
/**
* Whether the absolute encoder is integrated.
*/
private final boolean absoluteEncoder = false;
private final boolean absoluteEncoder = false;
/**
* Motion magic angle voltage setter.
*/
private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0);
private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0);
/**
* Velocity voltage setter for controlling drive motor.
*/
private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0);
private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0);
/**
* TalonFX motor controller.
*/
private final TalonFX motor;
private final TalonFX motor;
/**
* Conversion factor for the motor.
*/
private double conversionFactor;
private double conversionFactor;
/**
* Current TalonFX configuration.
*/
private TalonFXConfiguration configuration = new TalonFXConfiguration();
private TalonFXConfiguration configuration = new TalonFXConfiguration();
/**
* Current TalonFX Configurator.
*/
private TalonFXConfigurator cfg;
private TalonFXConfigurator cfg;
/**
@@ -60,12 +66,14 @@ public class TalonFXSwerve extends SwerveMotor
*
* @param motor Motor to use.
* @param isDriveMotor Whether this motor is a drive motor.
* @param motorType {@link DCMotor} which the {@link TalonFX} is attached to.
*/
public TalonFXSwerve(TalonFX motor, boolean isDriveMotor)
public TalonFXSwerve(TalonFX motor, boolean isDriveMotor, DCMotor motorType)
{
this.isDriveMotor = isDriveMotor;
this.motor = motor;
this.cfg = motor.getConfigurator();
this.simMotor = motorType;
factoryDefaults();
clearStickyFaults();
@@ -82,10 +90,11 @@ public class TalonFXSwerve extends SwerveMotor
* @param id ID of the TalonFX on the CANBus.
* @param canbus CANBus on which the TalonFX is on.
* @param isDriveMotor Whether the motor is a drive or steering motor.
* @param motorType {@link DCMotor} which the {@link TalonFX} is attached to.
*/
public TalonFXSwerve(int id, String canbus, boolean isDriveMotor)
public TalonFXSwerve(int id, String canbus, boolean isDriveMotor, DCMotor motorType)
{
this(new TalonFX(id, canbus), isDriveMotor);
this(new TalonFX(id, canbus), isDriveMotor, motorType);
}
/**
@@ -93,10 +102,11 @@ public class TalonFXSwerve extends SwerveMotor
*
* @param id ID of the TalonFX on the canbus.
* @param isDriveMotor Whether the motor is a drive or steering motor.
* @param motorType {@link DCMotor} which the {@link TalonFX} is attached to.
*/
public TalonFXSwerve(int id, boolean isDriveMotor)
public TalonFXSwerve(int id, boolean isDriveMotor, DCMotor motorType)
{
this(new TalonFX(id), isDriveMotor);
this(new TalonFX(id), isDriveMotor, motorType);
}
/**
@@ -181,63 +191,7 @@ public class TalonFXSwerve extends SwerveMotor
cfg.apply(configuration);
// Taken from democat's library.
// https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L16
configureCANStatusFrames(250);
}
/**
* Set the CAN status frames.
*
* @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information
*/
public void configureCANStatusFrames(int CANStatus1)
{
// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1);
}
/**
* Set the CAN status frames.
*
* @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information
* @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed Supply Current
* Measurement, Sticky Fault Information
* @param CANStatus3 Quadrature Information
* @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature
* @param CANStatus8 Pulse Width Information
* @param CANStatus10 Motion Profiling/Motion Magic Information
* @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1)
* @param CANStatus13 PID0 (Primary PID) Information
* @param CANStatus14 PID1 (Auxiliary PID) Information
* @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX)
* @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current Measurement
*/
public void configureCANStatusFrames(
int CANStatus1,
int CANStatus2,
int CANStatus3,
int CANStatus4,
int CANStatus8,
int CANStatus10,
int CANStatus12,
int CANStatus13,
int CANStatus14,
int CANStatus21,
int CANStatusCurrent)
{
// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1);
// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2);
// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3);
// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4);
// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8);
// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10);
// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12);
// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13);
// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14);
// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21);
// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current,
// CANStatusCurrent);
// TODO: Configure Status Frame 2 thru 21 if necessary
// https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods
// configureCANStatusFrames(250);
}
/**
@@ -290,7 +244,10 @@ public class TalonFXSwerve extends SwerveMotor
public void setInverted(boolean inverted)
{
// Timer.delay(1);
motor.setInverted(inverted);
cfg.refresh(configuration.MotorOutput);
configuration.MotorOutput.withInverted(
inverted ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive);
cfg.apply(configuration.MotorOutput);
}
/**
@@ -357,7 +314,7 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public double getVoltage()
{
return motor.getMotorVoltage().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue();
return motor.getMotorVoltage().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue().in(Volts);
}
/**
@@ -390,7 +347,7 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public double getVelocity()
{
return motor.getVelocity().getValue();
return motor.getVelocity().getValue().magnitude();
}
/**
@@ -401,7 +358,7 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public double getPosition()
{
return motor.getPosition().getValue();
return motor.getPosition().getValue().magnitude();
}
/**
@@ -414,8 +371,7 @@ public class TalonFXSwerve extends SwerveMotor
{
if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation)
{
position = position < 0 ? (position % 360) + 360 : position;
cfg.setPosition(position / 360);
cfg.setPosition(Degrees.of(position).in(Rotations));
}
}
@@ -441,8 +397,8 @@ public class TalonFXSwerve extends SwerveMotor
{
cfg.refresh(configuration.CurrentLimits);
cfg.apply(
configuration.CurrentLimits.withStatorCurrentLimit(currentLimit)
.withStatorCurrentLimitEnable(true));
configuration.CurrentLimits.withSupplyCurrentLimit(currentLimit)
.withSupplyCurrentLimitEnable(true));
}
/**
@@ -468,6 +424,21 @@ public class TalonFXSwerve extends SwerveMotor
return motor;
}
/**
* Get the {@link DCMotor} of the motor class.
*
* @return {@link DCMotor} of this type.
*/
@Override
public DCMotor getSimMotor()
{
if (simMotor == null)
{
simMotor = DCMotor.getKrakenX60(1);
}
return simMotor;
}
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*

56
swervelib/motors/TalonSRXSwerve.java
View File

@@ -7,6 +7,8 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.math.system.plant.DCMotor;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.math.SwerveMath;
import swervelib.parser.PIDFConfig;
@@ -21,42 +23,44 @@ public class TalonSRXSwerve extends SwerveMotor
/**
* Factory default already occurred.
*/
private final boolean factoryDefaultOccurred = false;
private final boolean factoryDefaultOccurred = false;
/**
* Current TalonFX configuration.
*/
private final TalonSRXConfiguration configuration = new TalonSRXConfiguration();
private final TalonSRXConfiguration configuration = new TalonSRXConfiguration();
/**
* Whether the absolute encoder is integrated.
*/
private final boolean absoluteEncoder = false;
private final boolean absoluteEncoder = false;
/**
* TalonSRX motor controller.
*/
private final WPI_TalonSRX motor;
private final WPI_TalonSRX motor;
/**
* The position conversion factor to convert raw sensor units to Meters Per 100ms, or Ticks to Degrees.
*/
private double positionConversionFactor = 1;
private double positionConversionFactor = 1;
/**
* If the TalonFX configuration has changed.
*/
private boolean configChanged = true;
private boolean configChanged = true;
/**
* Nominal voltage default to use with feedforward.
*/
private double nominalVoltage = 12.0;
private double nominalVoltage = 12.0;
/**
* Constructor for TalonSRX swerve motor.
*
* @param motor Motor to use.
* @param isDriveMotor Whether this motor is a drive motor.
* @param motorType {@link DCMotor} which the {@link WPI_TalonSRX} is attached to.
*/
public TalonSRXSwerve(WPI_TalonSRX motor, boolean isDriveMotor)
public TalonSRXSwerve(WPI_TalonSRX motor, boolean isDriveMotor, DCMotor motorType)
{
this.isDriveMotor = isDriveMotor;
this.motor = motor;
this.simMotor = motorType;
motor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder);
factoryDefaults();
@@ -69,10 +73,11 @@ public class TalonSRXSwerve extends SwerveMotor
*
* @param id ID of the TalonSRX on the canbus.
* @param isDriveMotor Whether the motor is a drive or steering motor.
* @param motorType {@link DCMotor} which the {@link TalonFX} is attached to.
*/
public TalonSRXSwerve(int id, boolean isDriveMotor)
public TalonSRXSwerve(int id, boolean isDriveMotor, DCMotor motorType)
{
this(new WPI_TalonSRX(id), isDriveMotor);
this(new WPI_TalonSRX(id), isDriveMotor, motorType);
}
/**
@@ -358,11 +363,6 @@ public class TalonSRXSwerve extends SwerveMotor
} else
{
var pos = motor.getSelectedSensorPosition() * positionConversionFactor;
pos %= 360;
if (pos < 360)
{
pos += 360;
}
return pos;
}
}
@@ -421,6 +421,17 @@ public class TalonSRXSwerve extends SwerveMotor
configChanged = true;
}
/**
* Set the selected feedback device for the TalonSRX.
*
* @param feedbackDevice Feedback device to select.
*/
public void setSelectedFeedbackDevice(FeedbackDevice feedbackDevice)
{
configuration.primaryPID.selectedFeedbackSensor = feedbackDevice;
configChanged = true;
}
/**
* Get the motor object from the module.
*
@@ -432,6 +443,21 @@ public class TalonSRXSwerve extends SwerveMotor
return motor;
}
/**
* Get the {@link DCMotor} of the motor class.
*
* @return {@link DCMotor} of this type.
*/
@Override
public DCMotor getSimMotor()
{
if (simMotor == null)
{
simMotor = DCMotor.getCIM(1);
}
return simMotor;
}
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*

3
swervelib/parser/Cache.java
View File

@@ -1,5 +1,6 @@
package swervelib.parser;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.RobotController;
import java.util.function.Supplier;
@@ -95,7 +96,7 @@ public class Cache<T>
*/
public T getValue()
{
if (isStale())
if (isStale() || RobotBase.isSimulation())
{
update();
}

2
swervelib/parser/SwerveControllerConfiguration.java
View File

@@ -18,7 +18,7 @@ public class SwerveControllerConfiguration
public final double
angleJoyStickRadiusDeadband; // Deadband for the minimum hypot for the heading joystick.
/**
* Maximum angular velocity in rad/s
* Maximum chassis angular velocity in rad/s
*/
public double maxAngularVelocity;

83
swervelib/parser/SwerveDriveConfiguration.java
View File

@@ -1,9 +1,14 @@
package swervelib.parser;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.system.plant.DCMotor;
import java.util.function.Supplier;
import org.ironmaple.simulation.drivesims.GyroSimulation;
import swervelib.SwerveModule;
import swervelib.imu.NavXSwerve;
import swervelib.imu.Pigeon2Swerve;
import swervelib.imu.SwerveIMU;
import swervelib.math.SwerveMath;
/**
* Swerve drive configurations used during SwerveDrive construction.
@@ -38,20 +43,18 @@ public class SwerveDriveConfiguration
* @param moduleConfigs Module configuration.
* @param swerveIMU Swerve IMU.
* @param invertedIMU Invert the IMU.
* @param driveFeedforward The drive motor feedforward to use for the {@link SwerveModule}.
* @param physicalCharacteristics {@link SwerveModulePhysicalCharacteristics} to store in association with self.
*/
public SwerveDriveConfiguration(
SwerveModuleConfiguration[] moduleConfigs,
SwerveIMU swerveIMU,
boolean invertedIMU,
SimpleMotorFeedforward driveFeedforward,
SwerveModulePhysicalCharacteristics physicalCharacteristics)
{
this.moduleCount = moduleConfigs.length;
this.imu = swerveIMU;
swerveIMU.setInverted(invertedIMU);
this.modules = createModules(moduleConfigs, driveFeedforward);
this.modules = createModules(moduleConfigs);
this.moduleLocationsMeters = new Translation2d[moduleConfigs.length];
for (SwerveModule module : modules)
{
@@ -63,17 +66,15 @@ public class SwerveDriveConfiguration
/**
* Create modules based off of the SwerveModuleConfiguration.
*
* @param swerves Swerve constants.
* @param driveFeedforward Drive feedforward created using
* {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}.
* @param swerves Swerve constants.
* @return Swerve Modules.
*/
public SwerveModule[] createModules(SwerveModuleConfiguration[] swerves, SimpleMotorFeedforward driveFeedforward)
public SwerveModule[] createModules(SwerveModuleConfiguration[] swerves)
{
SwerveModule[] modArr = new SwerveModule[swerves.length];
for (int i = 0; i < swerves.length; i++)
{
modArr[i] = new SwerveModule(i, swerves[i], driveFeedforward);
modArr[i] = new SwerveModule(i, swerves[i]);
}
return modArr;
}
@@ -96,4 +97,68 @@ public class SwerveDriveConfiguration
//Return Largest Radius
return centerOfModules.getDistance(moduleLocationsMeters[0]);
}
/**
* Get the trackwidth of the swerve modules.
*
* @return Effective trackwdtih in Meters
*/
public double getTrackwidth()
{
SwerveModuleConfiguration fr = SwerveMath.getSwerveModule(modules, true, false);
SwerveModuleConfiguration fl = SwerveMath.getSwerveModule(modules, true, true);
return fr.moduleLocation.getDistance(fl.moduleLocation);
}
/**
* Get the tracklength of the swerve modules.
*
* @return Effective tracklength in Meters
*/
public double getTracklength()
{
SwerveModuleConfiguration br = SwerveMath.getSwerveModule(modules, false, false);
SwerveModuleConfiguration bl = SwerveMath.getSwerveModule(modules, false, true);
return br.moduleLocation.getDistance(bl.moduleLocation);
}
/**
* Get the {@link DCMotor} corresponding to the first module's configuration.
*
* @return {@link DCMotor} of the drive motor.
*/
public DCMotor getDriveMotorSim()
{
SwerveModuleConfiguration fl = SwerveMath.getSwerveModule(modules, true, true);
return fl.driveMotor.getSimMotor();
}
/**
* Get the {@link DCMotor} corresponding to the first module configuration.
*
* @return {@link DCMotor} of the angle motor.
*/
public DCMotor getAngleMotorSim()
{
SwerveModuleConfiguration fl = SwerveMath.getSwerveModule(modules, true, true);
return fl.angleMotor.getSimMotor();
}
/**
* Get the gyro simulation for the robot.
*
* @return {@link GyroSimulation} gyro simulation.
*/
public Supplier<GyroSimulation> getGyroSim()
{
if (imu instanceof Pigeon2Swerve)
{
return GyroSimulation.getPigeon2();
} else if (imu instanceof NavXSwerve)
{
return GyroSimulation.getNav2X();
}
return GyroSimulation.getGeneric();
}
}

8
swervelib/parser/SwerveModuleConfiguration.java
View File

@@ -3,7 +3,7 @@ package swervelib.parser;
import edu.wpi.first.math.geometry.Translation2d;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.motors.SwerveMotor;
import swervelib.parser.json.MotorConfigDouble;
import swervelib.parser.json.modules.ConversionFactorsJson;
/**
* Swerve Module configuration class which is used to configure {@link swervelib.SwerveModule}.
@@ -17,7 +17,7 @@ public class SwerveModuleConfiguration
* {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} respectively to calculate the
* conversion factors.
*/
public final MotorConfigDouble conversionFactors;
public final ConversionFactorsJson conversionFactors;
/**
* Angle offset in degrees for the Swerve Module.
*/
@@ -89,7 +89,7 @@ public class SwerveModuleConfiguration
public SwerveModuleConfiguration(
SwerveMotor driveMotor,
SwerveMotor angleMotor,
MotorConfigDouble conversionFactors,
ConversionFactorsJson conversionFactors,
SwerveAbsoluteEncoder absoluteEncoder,
double angleOffset,
double xMeters,
@@ -139,7 +139,7 @@ public class SwerveModuleConfiguration
public SwerveModuleConfiguration(
SwerveMotor driveMotor,
SwerveMotor angleMotor,
MotorConfigDouble conversionFactors,
ConversionFactorsJson conversionFactors,
SwerveAbsoluteEncoder absoluteEncoder,
double angleOffset,
double xMeters,

46
swervelib/parser/SwerveModulePhysicalCharacteristics.java
View File

@@ -1,6 +1,6 @@
package swervelib.parser;
import swervelib.parser.json.MotorConfigDouble;
import swervelib.parser.json.modules.ConversionFactorsJson;
/**
* Configuration class which stores physical characteristics shared between every swerve module.
@@ -16,20 +16,32 @@ public class SwerveModulePhysicalCharacteristics
* The time it takes for the motor to go from 0 to full throttle in seconds.
*/
public final double driveMotorRampRate, angleMotorRampRate;
/**
* The minimum voltage to spin the module or wheel.
*/
public final double driveFrictionVoltage, angleFrictionVoltage;
/**
* Wheel grip tape coefficient of friction on carpet, as described by the vendor.
*/
public final double wheelGripCoefficientOfFriction;
public final double wheelGripCoefficientOfFriction;
/**
* Steer rotational inertia in (KilogramSquareMeters) kg/m_sq.
*/
public final double steerRotationalInertia;
/**
* Robot mass in Kilograms.
*/
public final double robotMassKg;
/**
* The voltage to use for the smart motor voltage compensation.
*/
public double optimalVoltage;
public double optimalVoltage;
/**
* The conversion factors for the drive and angle motors, created by
* {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and
* {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}.
*/
public MotorConfigDouble conversionFactor;
public ConversionFactorsJson conversionFactor;
/**
* Construct the swerve module physical characteristics.
@@ -47,15 +59,23 @@ public class SwerveModulePhysicalCharacteristics
* over drawing power from battery)
* @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents
* overdrawing power and power loss).
* @param angleFrictionVoltage Angle motor minimum voltage.
* @param driveFrictionVoltage Drive motor minimum voltage.
* @param steerRotationalInertia Steering rotational inertia in KilogramSquareMeters.
* @param robotMassKg Robot mass in kG.
*/
public SwerveModulePhysicalCharacteristics(
MotorConfigDouble conversionFactors,
ConversionFactorsJson conversionFactors,
double wheelGripCoefficientOfFriction,
double optimalVoltage,
int driveMotorCurrentLimit,
int angleMotorCurrentLimit,
double driveMotorRampRate,
double angleMotorRampRate)
double angleMotorRampRate,
double driveFrictionVoltage,
double angleFrictionVoltage,
double steerRotationalInertia,
double robotMassKg)
{
this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction;
this.optimalVoltage = optimalVoltage;
@@ -64,7 +84,7 @@ public class SwerveModulePhysicalCharacteristics
// Set the conversion factors to null if they are both 0.
if (conversionFactors != null)
{
if (conversionFactors.angle == 0 && conversionFactors.drive == 0)
if (conversionFactors.isAngleEmpty() && conversionFactors.isDriveEmpty())
{
this.conversionFactor = null;
}
@@ -74,6 +94,10 @@ public class SwerveModulePhysicalCharacteristics
this.angleMotorCurrentLimit = angleMotorCurrentLimit;
this.driveMotorRampRate = driveMotorRampRate;
this.angleMotorRampRate = angleMotorRampRate;
this.driveFrictionVoltage = driveFrictionVoltage;
this.angleFrictionVoltage = angleFrictionVoltage;
this.steerRotationalInertia = steerRotationalInertia;
this.robotMassKg = robotMassKg;
}
/**
@@ -90,7 +114,7 @@ public class SwerveModulePhysicalCharacteristics
* power and power loss).
*/
public SwerveModulePhysicalCharacteristics(
MotorConfigDouble conversionFactors,
ConversionFactorsJson conversionFactors,
double driveMotorRampRate,
double angleMotorRampRate)
{
@@ -101,6 +125,10 @@ public class SwerveModulePhysicalCharacteristics
40,
20,
driveMotorRampRate,
angleMotorRampRate);
angleMotorRampRate,
0.2,
0.3,
0.03,
50);
}
}

54
swervelib/parser/SwerveParser.java
View File

@@ -2,7 +2,7 @@ package swervelib.parser;
import com.fasterxml.jackson.databind.JsonNode;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import java.io.File;
import java.io.IOException;
import java.util.HashMap;
@@ -135,10 +135,7 @@ public class SwerveParser
*/
public SwerveDrive createSwerveDrive(double maxSpeed)
{
return createSwerveDrive(SwerveMath.createDriveFeedforward(physicalPropertiesJson.optimalVoltage,
maxSpeed,
physicalPropertiesJson.wheelGripCoefficientOfFriction),
maxSpeed);
return createSwerveDrive(maxSpeed, Pose2d.kZero);
}
/**
@@ -157,48 +154,20 @@ public class SwerveParser
*/
public SwerveDrive createSwerveDrive(double maxSpeed, double angleMotorConversionFactor, double driveMotorConversion)
{
physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor;
physicalPropertiesJson.conversionFactor.drive = driveMotorConversion;
return createSwerveDrive(SwerveMath.createDriveFeedforward(physicalPropertiesJson.optimalVoltage,
maxSpeed,
physicalPropertiesJson.wheelGripCoefficientOfFriction),
maxSpeed);
physicalPropertiesJson.conversionFactors.angle.factor = angleMotorConversionFactor;
physicalPropertiesJson.conversionFactors.drive.factor = driveMotorConversion;
return createSwerveDrive(maxSpeed, Pose2d.kZero);
}
/**
* Create {@link SwerveDrive} from JSON configuration directory.
*
* @param driveFeedforward Drive feedforward to use for swerve modules, should be created using
* {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double,
* double)}.
* @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration
* in {@link swervelib.SwerveController} of the robot.
* @param angleMotorConversionFactor Angle (AKA azimuth) motor conversion factor to convert motor controller PID loop
* units to degrees, usually created using
* {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)}.
* @param driveMotorConversion Drive motor conversion factor to convert motor controller PID loop units to
* meters per rotation, usually created using
* {@link SwerveMath#calculateMetersPerRotation(double, double, double)}.
* @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration in
* {@link swervelib.SwerveController} of the robot
* @param initialPose {@link Pose2d} initial pose.
* @return {@link SwerveDrive} instance.
*/
public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed,
double angleMotorConversionFactor, double driveMotorConversion)
{
physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor;
physicalPropertiesJson.conversionFactor.drive = driveMotorConversion;
return createSwerveDrive(driveFeedforward, maxSpeed);
}
/**
* Create {@link SwerveDrive} from JSON configuration directory.
*
* @param driveFeedforward Drive feedforward to use for swerve modules, should be created using
* {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}.
* @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration in
* {@link swervelib.SwerveController} of the robot
* @return {@link SwerveDrive} instance.
*/
public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed)
public SwerveDrive createSwerveDrive(double maxSpeed, Pose2d initialPose)
{
SwerveModuleConfiguration[] moduleConfigurations =
new SwerveModuleConfiguration[moduleJsons.length];
@@ -217,11 +186,12 @@ public class SwerveParser
moduleConfigurations,
swerveDriveJson.imu.createIMU(),
swerveDriveJson.invertedIMU,
driveFeedforward,
physicalPropertiesJson.createPhysicalProperties());
return new SwerveDrive(
swerveDriveConfiguration,
controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed), maxSpeed);
controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed),
maxSpeed,
initialPose);
}
}

72
swervelib/parser/json/DeviceJson.java
View File

@@ -4,11 +4,10 @@ import static swervelib.telemetry.SwerveDriveTelemetry.canIdWarning;
import static swervelib.telemetry.SwerveDriveTelemetry.i2cLockupWarning;
import static swervelib.telemetry.SwerveDriveTelemetry.serialCommsIssueWarning;
import com.revrobotics.SparkRelativeEncoder.Type;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.studica.frc.AHRS.NavXComType;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.SerialPort.Port;
import swervelib.encoders.AnalogAbsoluteEncoderSwerve;
import swervelib.encoders.CANCoderSwerve;
import swervelib.encoders.CanAndMagSwerve;
@@ -16,6 +15,7 @@ import swervelib.encoders.PWMDutyCycleEncoderSwerve;
import swervelib.encoders.SparkMaxAnalogEncoderSwerve;
import swervelib.encoders.SparkMaxEncoderSwerve;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.encoders.TalonSRXEncoderSwerve;
import swervelib.imu.ADIS16448Swerve;
import swervelib.imu.ADIS16470Swerve;
import swervelib.imu.ADXRS450Swerve;
@@ -27,6 +27,7 @@ import swervelib.imu.PigeonSwerve;
import swervelib.imu.SwerveIMU;
import swervelib.motors.SparkFlexSwerve;
import swervelib.motors.SparkMaxBrushedMotorSwerve;
import swervelib.motors.SparkMaxBrushedMotorSwerve.Type;
import swervelib.motors.SparkMaxSwerve;
import swervelib.motors.SwerveMotor;
import swervelib.motors.TalonFXSwerve;
@@ -92,6 +93,10 @@ public class DeviceJson
return new AnalogAbsoluteEncoderSwerve(id);
case "cancoder":
return new CANCoderSwerve(id, canbus != null ? canbus : "");
case "talonsrx_pwm":
return new TalonSRXEncoderSwerve(motor, FeedbackDevice.PulseWidthEncodedPosition);
case "talonsrx_analog":
return new TalonSRXEncoderSwerve(motor, FeedbackDevice.Analog);
default:
throw new RuntimeException(type + " is not a recognized absolute encoder type.");
}
@@ -122,7 +127,7 @@ public class DeviceJson
return new CanandgyroSwerve(id);
case "navx":
case "navx_spi":
return new NavXSwerve(SPI.Port.kMXP);
return new NavXSwerve(NavXComType.kMXP_SPI);
case "navx_i2c":
DriverStation.reportWarning(
"WARNING: There exists an I2C lockup issue on the roboRIO that could occur, more information here: " +
@@ -130,15 +135,15 @@ public class DeviceJson
".html#onboard-i2c-causing-system-lockups",
false);
i2cLockupWarning.set(true);
return new NavXSwerve(I2C.Port.kMXP);
return new NavXSwerve(NavXComType.kI2C);
case "navx_usb":
DriverStation.reportWarning("WARNING: There is issues when using USB camera's and the NavX like this!\n" +
"https://pdocs.kauailabs.com/navx-mxp/guidance/selecting-an-interface/", false);
serialCommsIssueWarning.set(true);
return new NavXSwerve(Port.kUSB);
return new NavXSwerve(NavXComType.kUSB1);
case "navx_mxp_serial":
serialCommsIssueWarning.set(true);
return new NavXSwerve(Port.kMXP);
return new NavXSwerve(NavXComType.kMXP_UART);
case "pigeon":
return new PigeonSwerve(id);
case "pigeon2":
@@ -162,39 +167,56 @@ public class DeviceJson
}
switch (type)
{
case "sparkmax_neo":
case "neo":
case "sparkmax":
return new SparkMaxSwerve(id, isDriveMotor, DCMotor.getNEO(1));
case "sparkmax_neo550":
case "neo550":
return new SparkMaxSwerve(id, isDriveMotor, DCMotor.getNeo550(1));
case "sparkflex_vortex":
case "vortex":
case "sparkflex":
return new SparkFlexSwerve(id, isDriveMotor, DCMotor.getNeoVortex(1));
case "sparkflex_neo":
return new SparkFlexSwerve(id, isDriveMotor, DCMotor.getNEO(1));
case "sparkflex_neo550":
return new SparkFlexSwerve(id, isDriveMotor, DCMotor.getNeo550(1));
case "falcon500":
case "falcon":
return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getFalcon500(1));
case "falcon500foc":
return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getFalcon500Foc(1));
case "krakenx60":
case "talonfx":
return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getKrakenX60(1));
case "krakenx60foc":
return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getKrakenX60Foc(1));
case "talonsrx":
return new TalonSRXSwerve(id, isDriveMotor, DCMotor.getCIM(1));
case "sparkmax_brushed":
switch (canbus)
{
case "greyhill_63r256":
return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, false);
return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, false, DCMotor.getCIM(1));
case "srx_mag_encoder":
return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, false);
return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, false, DCMotor.getCIM(1));
case "throughbore":
return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 8192, false);
return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 8192, false, DCMotor.getCIM(1));
case "throughbore_dataport":
return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 8192, true);
return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 8192, true, DCMotor.getCIM(1));
case "greyhill_63r256_dataport":
return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, true);
return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, true, DCMotor.getCIM(1));
case "srx_mag_encoder_dataport":
return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, true);
return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, true, DCMotor.getCIM(1));
default:
if (isDriveMotor)
{
throw new RuntimeException("Spark MAX " + id + " MUST have a encoder attached to the motor controller.");
}
// We are creating a motor for an angle motor which will use the absolute encoder attached to the data port.
return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 0, false);
return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 0, false, DCMotor.getCIM(1));
}
case "neo":
case "sparkmax":
return new SparkMaxSwerve(id, isDriveMotor);
case "sparkflex":
return new SparkFlexSwerve(id, isDriveMotor);
case "falcon":
case "talonfx":
return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor);
case "talonsrx":
return new TalonSRXSwerve(id, isDriveMotor);
default:
throw new RuntimeException(type + " is not a recognized motor type.");
}

67
swervelib/parser/json/ModuleJson.java
View File

@@ -1,8 +1,8 @@
package swervelib.parser.json;
import com.revrobotics.CANSparkMax;
import com.revrobotics.MotorFeedbackSensor;
import com.revrobotics.spark.SparkMax;
import edu.wpi.first.math.util.Units;
import swervelib.encoders.SparkMaxEncoderSwerve;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.motors.SwerveMotor;
import swervelib.parser.PIDFConfig;
@@ -26,14 +26,6 @@ public class ModuleJson
* Angle motor device configuration.
*/
public DeviceJson angle;
/**
* Conversion factor for the module, if different from the one in swervedrive.json
* <p>
* Conversion factor applied to the motor controllers PID loops. Can be calculated with
* {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors or
* {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for drive motors.
*/
public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0);
/**
* Conversion Factors composition. Auto-calculates the conversion factors.
*/
@@ -81,37 +73,8 @@ public class ModuleJson
SwerveMotor angleMotor = angle.createMotor(false);
SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor);
// Setup deprecation notice.
// if (this.conversionFactor.drive != 0 && this.conversionFactor.angle != 0)
// {
// new Alert("Configuration",
// "\n'conversionFactor': {'drive': " + conversionFactor.drive + ", 'angle': " + conversionFactor.angle +
// "} \nis deprecated, please use\n" +
// "'conversionFactors': {'drive': {'factor': " + conversionFactor.drive + "}, 'angle': {'factor': " +
// conversionFactor.angle + "} }",
// AlertType.WARNING).set(true);
// }
// Override with composite conversion factor.
if (!conversionFactors.isAngleEmpty())
{
conversionFactor.angle = conversionFactors.angle.calculate();
}
if (!conversionFactors.isDriveEmpty())
{
conversionFactor.drive = conversionFactors.drive.calculate();
}
// Set the conversion factors to null if they are both 0.
if (this.conversionFactor != null)
{
if (this.conversionFactor.angle == 0 && this.conversionFactor.drive == 0)
{
this.conversionFactor = null;
}
}
if (this.conversionFactor == null && physicalCharacteristics.conversionFactor == null)
if (!conversionFactors.works() && physicalCharacteristics.conversionFactor == null)
{
throw new RuntimeException("No Conversion Factor configured! Please create SwerveDrive using \n" +
"SwerveParser.createSwerveDrive(driveFeedforward, maxSpeed, angleMotorConversionFactor, driveMotorConversion)\n" +
@@ -120,27 +83,27 @@ public class ModuleJson
"OR\n" +
"Set the conversion factor in physicalproperties.json OR the module JSON file." +
"REMEMBER: You can calculate the conversion factors using SwerveMath.calculateMetersPerRotation AND SwerveMath.calculateDegreesPerSteeringRotation\n");
} else if (physicalCharacteristics.conversionFactor != null && this.conversionFactor == null)
} else if (physicalCharacteristics.conversionFactor.works() && !conversionFactors.works())
{
this.conversionFactor = physicalCharacteristics.conversionFactor;
} else if (physicalCharacteristics.conversionFactor !=
null) // If both are defined, override 0 with the physical characterstics input.
conversionFactors = physicalCharacteristics.conversionFactor;
} else if (physicalCharacteristics.conversionFactor.works())
// If both are defined, override 0 with the physical characterstics input.
{
this.conversionFactor.angle = this.conversionFactor.angle == 0 ? physicalCharacteristics.conversionFactor.angle
: this.conversionFactor.angle;
this.conversionFactor.drive = this.conversionFactor.drive == 0 ? physicalCharacteristics.conversionFactor.drive
: this.conversionFactor.drive;
conversionFactors.angle = conversionFactors.isAngleEmpty() ? physicalCharacteristics.conversionFactor.angle
: conversionFactors.angle;
conversionFactors.drive = conversionFactors.isDriveEmpty() ? physicalCharacteristics.conversionFactor.drive
: conversionFactors.drive;
}
if (this.conversionFactor.drive == 0 || this.conversionFactor.angle == 0)
if (conversionFactors.isDriveEmpty() || conversionFactors.isAngleEmpty())
{
throw new RuntimeException(
"Conversion factors cannot be 0, please configure conversion factors in physicalproperties.json or the module JSON files.");
}
// Backwards compatibility, auto-optimization.
if (conversionFactor.angle == 360 && absEncoder != null &&
absEncoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor && angleMotor.getMotor() instanceof CANSparkMax)
if (conversionFactors.angle.factor == 360 && absEncoder != null &&
absEncoder instanceof SparkMaxEncoderSwerve && angleMotor.getMotor() instanceof SparkMax)
{
angleMotor.setAbsoluteEncoder(absEncoder);
}
@@ -148,7 +111,7 @@ public class ModuleJson
return new SwerveModuleConfiguration(
drive.createMotor(true),
angleMotor,
conversionFactor,
conversionFactors,
absEncoder,
absoluteEncoderOffset,
Units.inchesToMeters(Math.round(location.x) == 0 ? location.front : location.x),

57
swervelib/parser/json/PhysicalPropertiesJson.java
View File

@@ -1,5 +1,8 @@
package swervelib.parser.json;
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import swervelib.parser.SwerveModulePhysicalCharacteristics;
import swervelib.parser.json.modules.ConversionFactorsJson;
@@ -9,13 +12,23 @@ import swervelib.parser.json.modules.ConversionFactorsJson;
public class PhysicalPropertiesJson
{
/**
* Conversion factor applied to the motor controllers PID loops. Can be calculated with
* {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors or
* {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for drive motors.
* DEPRECATED! Use {@link PhysicalPropertiesJson#conversionFactors} instead.
*/
public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0);
@Deprecated(since = "2025", forRemoval = true)
public MotorConfigDouble conversionFactor = new MotorConfigDouble();
/**
* Minimum voltage to spin the module or wheel.
*/
public MotorConfigDouble friction = new MotorConfigDouble(0.3, 0.2);
/**
* Steer rotational inertia in KilogramMetersSquare.
*/
public double steerRotationalInertia = 0.03;
/**
* Robot mass in lb (pounds)
*/
public double robotMass = 110.2311;
/**
* Conversion Factors composition. Auto-calculates the conversion factors.
*/
@@ -45,35 +58,29 @@ public class PhysicalPropertiesJson
public SwerveModulePhysicalCharacteristics createPhysicalProperties()
{
// Setup deprecation notice.
// if (conversionFactor.drive != 0 && conversionFactor.angle != 0 && conversionFactors.isDriveEmpty() &&
// conversionFactors.isAngleEmpty())
// {
// new Alert("Configuration",
// "\n'conversionFactor': {'drive': " + conversionFactor.drive + ", 'angle': " + conversionFactor.angle +
// "} \nis deprecated, please use\n" +
// "'conversionFactors': {'drive': {'factor': " + conversionFactor.drive + "}, 'angle': {'factor': " +
// conversionFactor.angle + "} }",
// AlertType.ERROR_TRACE).set(true);
// }
if (!conversionFactors.isAngleEmpty())
if (conversionFactor.drive != 0 && conversionFactor.angle != 0 && conversionFactors.isDriveEmpty() &&
conversionFactors.isAngleEmpty())
{
conversionFactor.angle = conversionFactors.angle.calculate();
}
if (!conversionFactors.isDriveEmpty())
{
conversionFactor.drive = conversionFactors.drive.calculate();
new Alert("Configuration",
"\n'conversionFactor': {'drive': " + conversionFactor.drive + ", 'angle': " + conversionFactor.angle +
"} \nis deprecated, please use\n" +
"'conversionFactors': {'drive': {'factor': " + conversionFactor.drive + "}, 'angle': {'factor': " +
conversionFactor.angle + "} }",
AlertType.kError).set(true);
}
return new SwerveModulePhysicalCharacteristics(
conversionFactor,
conversionFactors,
wheelGripCoefficientOfFriction,
optimalVoltage,
currentLimit.drive,
currentLimit.angle,
rampRate.drive,
rampRate.angle);
rampRate.angle,
friction.drive,
friction.angle,
steerRotationalInertia,
Units.Pounds.of(robotMass).in(Units.Kilogram));
}
}

12
swervelib/parser/json/modules/AngleConversionFactorsJson.java
View File

@@ -1,8 +1,6 @@
package swervelib.parser.json.modules;
import swervelib.math.SwerveMath;
import swervelib.telemetry.Alert;
import swervelib.telemetry.Alert.AlertType;
/**
* Angle motor conversion factors composite JSON parse class.
@@ -13,11 +11,11 @@ public class AngleConversionFactorsJson
/**
* Gear ratio for the angle/steering/azimuth motor on the Swerve Module. Motor rotations to 1 wheel rotation.
*/
public double gearRatio = 0;
public double gearRatio;
/**
* Calculated or given conversion factor.
*/
public double factor = 0;
public double factor = 0;
/**
* Calculate the drive conversion factor.
@@ -26,12 +24,6 @@ public class AngleConversionFactorsJson
*/
public double calculate()
{
if (factor != 0 && gearRatio != 0)
{
new Alert("Configuration",
"The given angle conversion factor takes precedence over the composite conversion factor, please remove 'factor' if you want to use the composite factor instead.",
AlertType.WARNING).set(true);
}
if (factor == 0)
{
factor = SwerveMath.calculateDegreesPerSteeringRotation(gearRatio);

17
swervelib/parser/json/modules/ConversionFactorsJson.java
View File

@@ -22,7 +22,8 @@ public class ConversionFactorsJson
*/
public boolean isDriveEmpty()
{
return drive.factor == 0 && drive.diameter == 0 && drive.gearRatio == 0;
drive.calculate();
return drive.factor == 0;
}
/**
@@ -32,6 +33,18 @@ public class ConversionFactorsJson
*/
public boolean isAngleEmpty()
{
return angle.factor == 0 && angle.gearRatio == 0;
angle.calculate();
return angle.factor == 0;
}
/**
* Check if the conversion factor can be found.
*
* @return If the conversion factors can be found.
*/
public boolean works()
{
return (angle.factor != 0 && drive.factor != 0) ||
((drive.gearRatio != 0 && drive.diameter != 0)) && (angle.gearRatio != 0);
}
}

14
swervelib/parser/json/modules/DriveConversionFactorsJson.java
View File

@@ -2,8 +2,6 @@ package swervelib.parser.json.modules;
import edu.wpi.first.math.util.Units;
import swervelib.math.SwerveMath;
import swervelib.telemetry.Alert;
import swervelib.telemetry.Alert.AlertType;
/**
* Drive motor composite JSON parse class.
@@ -14,15 +12,15 @@ public class DriveConversionFactorsJson
/**
* Gear ratio for the drive motor rotations to turn the wheel 1 complete rotation.
*/
public double gearRatio = 0;
public double gearRatio;
/**
* Diameter of the wheel in inches.
*/
public double diameter = 0;
public double diameter;
/**
* Calculated conversion factor.
*/
public double factor = 0;
public double factor = 0;
/**
* Calculate the drive conversion factor.
@@ -31,12 +29,6 @@ public class DriveConversionFactorsJson
*/
public double calculate()
{
if (factor != 0 && (diameter != 0 || gearRatio != 0))
{
new Alert("Configuration",
"The given drive conversion factor takes precedence over the composite conversion factor, please remove 'factor' if you want to use the composite factor instead.",
AlertType.WARNING).set(true);
}
if (factor == 0)
{
factor = SwerveMath.calculateMetersPerRotation(Units.inchesToMeters(this.diameter), this.gearRatio);

32
swervelib/simulation/SwerveIMUSimulation.java
View File

@@ -6,9 +6,9 @@ import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import java.util.Optional;
import org.ironmaple.simulation.drivesims.GyroSimulation;
/**
* Simulation for {@link swervelib.SwerveDrive} IMU.
@@ -16,27 +16,16 @@ import java.util.Optional;
public class SwerveIMUSimulation
{
/**
* Main timer to control movement estimations.
*/
private final Timer timer;
/**
* The last time the timer was read, used to determine position changes.
*/
private double lastTime;
/**
* Heading of the robot.
*/
private double angle;
private final GyroSimulation gyroSimulation;
/**
* Create the swerve drive IMU simulation.
*
* @param gyroSimulation Gyro simulation from MapleSim.
*/
public SwerveIMUSimulation()
public SwerveIMUSimulation(GyroSimulation gyroSimulation)
{
timer = new Timer();
timer.start();
lastTime = timer.get();
this.gyroSimulation = gyroSimulation;
}
/**
@@ -46,7 +35,7 @@ public class SwerveIMUSimulation
*/
public Rotation2d getYaw()
{
return new Rotation2d(angle);
return gyroSimulation.getGyroReading();
}
/**
@@ -76,7 +65,7 @@ public class SwerveIMUSimulation
*/
public Rotation3d getGyroRotation3d()
{
return new Rotation3d(0, 0, angle);
return new Rotation3d(0, 0, getYaw().getRadians());
}
/**
@@ -104,9 +93,6 @@ public class SwerveIMUSimulation
Pose2d[] modulePoses,
Field2d field)
{
angle += kinematics.toChassisSpeeds(states).omegaRadiansPerSecond * (timer.get() - lastTime);
lastTime = timer.get();
field.getObject("XModules").setPoses(modulePoses);
}
@@ -117,6 +103,6 @@ public class SwerveIMUSimulation
*/
public void setAngle(double angle)
{
this.angle = angle;
this.gyroSimulation.setRotation(Rotation2d.fromRadians(angle));
}
}

68
swervelib/simulation/SwerveModuleSimulation.java
View File

@@ -1,54 +1,33 @@
package swervelib.simulation;
import static edu.wpi.first.units.Units.Amps;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.Timer;
import org.ironmaple.simulation.drivesims.SelfControlledSwerveDriveSimulation;
import swervelib.parser.SwerveModulePhysicalCharacteristics;
/**
* Class to hold simulation data for {@link swervelib.SwerveModule}
* Class that wraps around {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation}
*/
public class SwerveModuleSimulation
{
private SelfControlledSwerveDriveSimulation.SelfControlledModuleSimulation mapleSimModule = null;
/**
* Main timer to simulate the passage of time.
* Configure the maple sim module
*
* @param simModule the {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} object for simulation
*/
private final Timer timer;
/**
* Time delta since last update
*/
private double dt;
/**
* Fake motor position.
*/
private double fakePos;
/**
* The fake speed of the previous state, used to calculate {@link SwerveModuleSimulation#fakePos}.
*/
private double fakeSpeed;
/**
* Last time queried.
*/
private double lastTime;
/**
* Current simulated swerve module state.
*/
private SwerveModuleState state;
/**
* Create simulation class and initialize module at 0.
*/
public SwerveModuleSimulation()
public void configureSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSimulation simModule,
SwerveModulePhysicalCharacteristics physicalCharacteristics)
{
timer = new Timer();
timer.start();
lastTime = timer.get();
state = new SwerveModuleState(0, Rotation2d.fromDegrees(0));
fakeSpeed = 0;
fakePos = 0;
dt = 0;
this.mapleSimModule = new SelfControlledSwerveDriveSimulation.SelfControlledModuleSimulation(simModule);
this.mapleSimModule.withCurrentLimits(
Amps.of(physicalCharacteristics.driveMotorCurrentLimit),
Amps.of(physicalCharacteristics.angleMotorCurrentLimit));
}
/**
@@ -59,13 +38,7 @@ public class SwerveModuleSimulation
*/
public void updateStateAndPosition(SwerveModuleState desiredState)
{
dt = timer.get() - lastTime;
lastTime = timer.get();
state = desiredState;
fakeSpeed = desiredState.speedMetersPerSecond;
fakePos += (fakeSpeed * dt);
mapleSimModule.runModuleState(desiredState);
}
/**
@@ -75,8 +48,7 @@ public class SwerveModuleSimulation
*/
public SwerveModulePosition getPosition()
{
return new SwerveModulePosition(fakePos, state.angle);
return mapleSimModule.getModulePosition();
}
/**
@@ -86,6 +58,12 @@ public class SwerveModuleSimulation
*/
public SwerveModuleState getState()
{
if (mapleSimModule == null)
{
return new SwerveModuleState();
}
SwerveModuleState state = mapleSimModule.getMeasuredState();
state.angle = state.angle.minus(Rotation2d.kZero);
return state;
}
}

179
swervelib/telemetry/SwerveDriveTelemetry.java
View File

@@ -1,8 +1,15 @@
package swervelib.telemetry;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StructArrayPublisher;
import edu.wpi.first.networktables.StructPublisher;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import swervelib.telemetry.Alert.AlertType;
/**
* Telemetry to describe the {@link swervelib.SwerveDrive} following frc-web-components. (Which follows AdvantageKit)
@@ -13,90 +20,186 @@ public class SwerveDriveTelemetry
/**
* An {@link Alert} for if the CAN ID is greater than 40.
*/
public static final Alert canIdWarning = new Alert("JSON",
"CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!",
Alert.AlertType.WARNING);
public static final Alert canIdWarning = new Alert("JSON",
"CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!",
AlertType.kWarning);
/**
* An {@link Alert} for if there is an I2C lockup issue on the roboRIO.
*/
public static final Alert i2cLockupWarning = new Alert("IMU",
"I2C lockup issue detected on roboRIO. Check console for more information.",
Alert.AlertType.WARNING);
public static final Alert i2cLockupWarning = new Alert("IMU",
"I2C lockup issue detected on roboRIO. Check console for more information.",
AlertType.kWarning);
/**
* NavX serial comm issue.
*/
public static final Alert serialCommsIssueWarning = new Alert("IMU",
"Serial comms is interrupted with USB and other serial traffic and causes intermittent connected/disconnection issues. Please consider another protocol or be mindful of this.",
AlertType.WARNING);
public static final Alert serialCommsIssueWarning = new Alert("IMU",
"Serial comms is interrupted with USB and other serial traffic and causes intermittent connected/disconnection issues. Please consider another protocol or be mindful of this.",
AlertType.kWarning);
/**
* The current telemetry verbosity level.
* Measured swerve module states object.
*/
public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE;
public static SwerveModuleState[] measuredStatesObj = new SwerveModuleState[4];
/**
* State of simulation of the Robot, used to optimize retrieval.
* Desired swerve module states object
*/
public static boolean isSimulation = RobotBase.isSimulation();
public static SwerveModuleState[] desiredStatesObj = new SwerveModuleState[4];
/**
* The number of swerve modules
* The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the
* chassis speeds properties.
*/
public static int moduleCount;
public static ChassisSpeeds measuredChassisSpeedsObj = new ChassisSpeeds();
/**
* The Locations of the swerve drive wheels.
* Describes the desired forward, sideways and angular velocity of the robot.
*/
public static double[] wheelLocations;
/**
* An array of rotation and velocity values describing the measured state of each swerve module
*/
public static double[] measuredStates;
/**
* An array of rotation and velocity values describing the desired state of each swerve module
*/
public static double[] desiredStates;
public static ChassisSpeeds desiredChassisSpeedsObj = new ChassisSpeeds();
/**
* The robot's current rotation based on odometry or gyro readings
*/
public static double robotRotation = 0;
public static Rotation2d robotRotationObj = new Rotation2d();
/**
* The current telemetry verbosity level.
*/
public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE;
/**
* State of simulation of the Robot, used to optimize retrieval.
*/
public static boolean isSimulation = RobotBase.isSimulation();
/**
* The number of swerve modules
*/
public static int moduleCount;
/**
* The Locations of the swerve drive wheels.
*/
public static double[] wheelLocations;
/**
* An array of rotation and velocity values describing the measured state of each swerve module
*/
public static double[] measuredStates;
/**
* An array of rotation and velocity values describing the desired state of each swerve module
*/
public static double[] desiredStates;
/**
* The robot's current rotation based on odometry or gyro readings
*/
public static double robotRotation = 0;
/**
* The maximum achievable speed of the modules, used to adjust the size of the vectors.
*/
public static double maxSpeed;
public static double maxSpeed;
/**
* The units of the module rotations and robot rotation
*/
public static String rotationUnit = "degrees";
public static String rotationUnit = "degrees";
/**
* The distance between the left and right modules.
*/
public static double sizeLeftRight;
public static double sizeLeftRight;
/**
* The distance between the front and back modules.
*/
public static double sizeFrontBack;
public static double sizeFrontBack;
/**
* The direction the robot should be facing when the "Robot Rotation" is zero or blank. This option is often useful to
* align with odometry data or match videos. 'up', 'right', 'down' or 'left'
*/
public static String forwardDirection = "up";
public static String forwardDirection = "up";
/**
* The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the
* chassis speeds properties.
*/
public static double maxAngularVelocity;
public static double maxAngularVelocity;
/**
* The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the
* chassis speeds properties.
*/
public static double[] measuredChassisSpeeds = new double[3];
public static double[] measuredChassisSpeeds = new double[3];
/**
* Describes the desired forward, sideways and angular velocity of the robot.
*/
public static double[] desiredChassisSpeeds = new double[3];
public static double[] desiredChassisSpeeds = new double[3];
/**
* Struct publisher for AdvantageScope swerve widgets.
*/
private static StructArrayPublisher<SwerveModuleState> measuredStatesStruct
= NetworkTableInstance.getDefault()
.getStructArrayTopic(
"swerve/advantagescope/currentStates",
SwerveModuleState.struct)
.publish();
/**
* Struct publisher for AdvantageScope swerve widgets.
*/
private static StructArrayPublisher<SwerveModuleState> desiredStatesStruct
= NetworkTableInstance.getDefault()
.getStructArrayTopic(
"swerve/advantagescope/desiredStates",
SwerveModuleState.struct)
.publish();
/**
* Measured {@link ChassisSpeeds} for NT4 AdvantageScope swerve widgets.
*/
private static StructPublisher<ChassisSpeeds> measuredChassisSpeedsStruct
= NetworkTableInstance.getDefault()
.getStructTopic(
"swerve/advantagescope/measuredChassisSpeeds",
ChassisSpeeds.struct)
.publish();
/**
* Desired {@link ChassisSpeeds} for NT4 AdvantageScope swerve widgets.
*/
private static StructPublisher<ChassisSpeeds> desiredChassisSpeedsStruct
= NetworkTableInstance.getDefault()
.getStructTopic(
"swerve/advantagescope/desiredChassisSpeeds",
ChassisSpeeds.struct)
.publish();
/**
* Robot {@link Rotation2d} for AdvantageScope swerve widgets.
*/
private static StructPublisher<Rotation2d> robotRotationStruct
= NetworkTableInstance.getDefault()
.getStructTopic(
"swerve/advantagescope/robotRotation",
Rotation2d.struct)
.publish();
/**
* Upload data to smartdashboard
*/
public static void updateData()
{
measuredChassisSpeeds[0] = measuredChassisSpeedsObj.vxMetersPerSecond;
measuredChassisSpeeds[1] = measuredChassisSpeedsObj.vxMetersPerSecond;
measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeedsObj.omegaRadiansPerSecond);
desiredChassisSpeeds[0] = desiredChassisSpeedsObj.vxMetersPerSecond;
desiredChassisSpeeds[1] = desiredChassisSpeedsObj.vyMetersPerSecond;
desiredChassisSpeeds[2] = Math.toDegrees(desiredChassisSpeedsObj.omegaRadiansPerSecond);
robotRotation = robotRotationObj.getDegrees();
for (int i = 0; i < measuredStatesObj.length; i++)
{
SwerveModuleState state = measuredStatesObj[i];
if (state != null)
{
measuredStates[i * 2] = state.angle.getDegrees();
measuredStates[i * 2 + 1] = state.speedMetersPerSecond;
}
}
for (int i = 0; i < desiredStatesObj.length; i++)
{
SwerveModuleState state = desiredStatesObj[i];
if (state != null)
{
desiredStates[i * 2] = state.angle.getDegrees();
desiredStates[i * 2 + 1] = state.speedMetersPerSecond;
}
}
SmartDashboard.putNumber("swerve/moduleCount", moduleCount);
SmartDashboard.putNumberArray("swerve/wheelLocations", wheelLocations);
SmartDashboard.putNumberArray("swerve/measuredStates", measuredStates);
@@ -110,6 +213,12 @@ public class SwerveDriveTelemetry
SmartDashboard.putNumber("swerve/maxAngularVelocity", maxAngularVelocity);
SmartDashboard.putNumberArray("swerve/measuredChassisSpeeds", measuredChassisSpeeds);
SmartDashboard.putNumberArray("swerve/desiredChassisSpeeds", desiredChassisSpeeds);
desiredStatesStruct.set(desiredStatesObj);
measuredStatesStruct.set(measuredStatesObj);
desiredChassisSpeedsStruct.set(desiredChassisSpeedsObj);
measuredChassisSpeedsStruct.set(measuredChassisSpeedsObj);
robotRotationStruct.set(robotRotationObj);
}
/**