diff --git a/swervelib/SwerveController.java b/swervelib/SwerveController.java index 057ba46a..80116de6 100644 --- a/swervelib/SwerveController.java +++ b/swervelib/SwerveController.java @@ -227,7 +227,7 @@ public double headingCalculate(double currentHeadingAngleRadians, double targetH * * @param angularVelocity Angular velocity in radians per second. */ - public void setMaximumAngularVelocity(double angularVelocity) + public void setMaximumChassisAngularVelocity(double angularVelocity) { config.maxAngularVelocity = angularVelocity; } diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java index 7d83e5d0..f0acfc6f 100644 --- a/swervelib/SwerveDrive.java +++ b/swervelib/SwerveDrive.java @@ -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.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.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.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 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 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 @@ private void checkIfTunerXCompatible() public void setOdometryPeriod(double period) { odometryThread.stop(); + SimulatedArena.overrideSimulationTimings(Seconds.of(period), 1); odometryThread.startPeriodic(period); } @@ -309,6 +374,7 @@ public void setOdometryPeriod(double period) public void stopOdometryThread() { odometryThread.stop(); + SimulatedArena.overrideSimulationTimings(Seconds.of(TimedRobot.kDefaultPeriod), 5); } /** @@ -381,9 +447,8 @@ public void setHeadingCorrection(boolean state, double deadband) 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 void driveFieldOrientedandRobotOriented(ChassisSpeeds fieldOrientedVeloci */ public void driveFieldOriented(ChassisSpeeds velocity) { - ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading()); - drive(fieldOrientedVelocity); + velocity.toRobotRelativeSpeeds(getOdometryHeading()); + drive(velocity); } /** @@ -405,8 +470,8 @@ public void driveFieldOriented(ChassisSpeeds velocity) */ 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 void drive( { // 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 void drive( { // 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 void drive( * 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 void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent } // 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 void setMaximumSpeeds( /** * 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, 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 Math.max(this.attainableMaxTranslationalSpeedMetersPerSecond, maxSpeedMPS); + 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 double getMaximumVelocity() * * @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 @@ private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds 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 @@ private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds /** * 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) @@ -634,27 +713,68 @@ public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoo } } + /** + * Drive the robot using the {@link SwerveModuleState}, it is recommended to have + * {@link SwerveDrive#setCosineCompensator(boolean)} set to false for this.
+ *

+ * + * @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 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; + + // 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 + ); + } + } + /** * Set chassis speeds with closed-loop velocity control. * - * @param chassisSpeeds Chassis speeds to set. + * @param robotRelativeSpeeds Chassis speeds to set. */ - public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) + public void setChassisSpeeds(ChassisSpeeds robotRelativeSpeeds) { - chassisSpeeds = movementOptimizations(chassisSpeeds, - autonomousChassisVelocityCorrection, - autonomousAngularVelocityCorrection); + robotRelativeSpeeds = movementOptimizations(robotRelativeSpeeds, + autonomousChassisVelocityCorrection, + autonomousAngularVelocityCorrection); - SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond); + SwerveDriveTelemetry.desiredChassisSpeedsObj = robotRelativeSpeeds; - setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), chassisSpeeds, false); + setRawModuleStates(kinematics.toSwerveModuleStates(robotRelativeSpeeds), robotRelativeSpeeds, false); } /** - * Gets the current pose (position and rotation) of the robot, as reported by odometry. + * Gets the measured pose (position and rotation) of the robot, as reported by odometry. * * @return The robot's pose */ @@ -668,7 +788,38 @@ public Pose2d getPose() } /** - * 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 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 getSimulationDriveTrainPose() + { + odometryLock.lock(); + Optional 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 ChassisSpeeds getFieldVelocity() { // 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 void resetOdometry(Pose2d pose) { 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 void setModuleEncoderAutoSynchronize(boolean enabled, double deadband) } - /** - * 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 void lockPose() 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 void replaceSwerveModuleFeedforward(SimpleMotorFeedforward driveFeedforwa public void updateOdometry() { odometryLock.lock(); + invalidateCache(); try { // Update odometry swerveDrivePoseEstimator.update(getYaw(), getModulePositions()); - // Update angle accumulator if the robot is simulated - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) + if (SwerveDriveTelemetry.isSimulation) { - Pose2d[] modulePoses = getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition()); - if (SwerveDriveTelemetry.isSimulation) + try + { + SimulatedArena.getInstance().simulationPeriodic(); + } catch (Exception e) { - simIMU.updateOdometry( - kinematics, - getStates(), - modulePoses, - field); + DriverStation.reportError("MapleSim error", false); } + } - ChassisSpeeds measuredChassisSpeeds = getRobotVelocity(); - SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond; - SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond; - SwerveDriveTelemetry.measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond); - SwerveDriveTelemetry.robotRotation = getOdometryHeading().getDegrees(); + // Update angle accumulator if the robot is simulated + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) + { + 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 void updateOdometry() } 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 void updateOdometry() 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 void setCosineCompensator(boolean enabled) /** * 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 void setChassisDiscretization(boolean enable, double dtSeconds) * 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 void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAut /** * 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); + } } diff --git a/swervelib/SwerveDriveTest.java b/swervelib/SwerveDriveTest.java index a3d71dfe..70405a3e 100644 --- a/swervelib/SwerveDriveTest.java +++ b/swervelib/SwerveDriveTest.java @@ -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 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 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> 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 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> 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 static double findCouplingRatio(SwerveDrive swerveDrive, double volts, bo 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 static double findCouplingRatio(SwerveDrive swerveDrive, double volts, bo 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 static SysIdRoutine setDriveSysIdRoutine(Config config, SubsystemBase swe SwerveDrive swerveDrive, double maxVolts) { return new SysIdRoutine(config, new SysIdRoutine.Mechanism( - (Measure voltage) -> { + (Voltage voltage) -> { SwerveDriveTest.centerModules(swerveDrive); SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, Math.min(voltage.in(Volts), maxVolts)); }, @@ -380,7 +381,7 @@ public static SysIdRoutine setAngleSysIdRoutine(Config config, SubsystemBase swe SwerveDrive swerveDrive) { return new SysIdRoutine(config, new SysIdRoutine.Mechanism( - (Measure voltage) -> { + (Voltage voltage) -> { SwerveDriveTest.powerAngleMotorsVoltage(swerveDrive, voltage.in(Volts)); SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0); }, diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java index 8b5dec10..97967f24 100644 --- a/swervelib/SwerveModule.java +++ b/swervelib/SwerveModule.java @@ -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. + */ + private LinearVelocity maxDriveVelocity; + /** + * Maximum {@link AngularVelocity} for the azimuth/angle motor of the swerve module. */ - public double maxSpeed; + 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 SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat 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 SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat 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 SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat } // 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 SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat 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 SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat 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 SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat 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 void setAnglePIDF(PIDFConfig config) */ 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.
WARNING: If you are not using one of the functions from + * {@link SwerveDrive} you may screw up {@link SwerveDrive#kinematics} + * + * @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 void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, 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 void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, * @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 @@ private double getCosineCompensatedVelocity(SwerveModuleState desiredState) cosineScalar = 1; } - return desiredState.speedMetersPerSecond * (cosineScalar); + return MetersPerSecond.of(desiredState.speedMetersPerSecond).times(cosineScalar); } /** @@ -518,6 +559,13 @@ public double getAbsolutePosition() */ 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 void pushOffsetsToEncoders() 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 boolean getAbsoluteEncoderReadIssue() } } + + /** + * 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 void updateTelemetry() 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, null 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); + } } diff --git a/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java b/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java index fbd8214f..11016f65 100644 --- a/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java +++ b/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java @@ -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 AnalogAbsoluteEncoderSwerve(AnalogInput encoder) 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); } /** diff --git a/swervelib/encoders/CANCoderSwerve.java b/swervelib/encoders/CANCoderSwerve.java index 24ca2013..0477b240 100644 --- a/swervelib/encoders/CANCoderSwerve.java +++ b/swervelib/encoders/CANCoderSwerve.java @@ -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 CANCoderSwerve(int id, String canbus) 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 void configure(boolean inverted) 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 double getAbsolutePosition() readingFaulty.set(false); } - StatusSignal angle = encoder.getAbsolutePosition(); + StatusSignal 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 double getAbsolutePosition() readingIgnored.set(false); } - return angle.getValue() * 360; + return angle.getValue().in(Degrees); } /** @@ -213,6 +218,6 @@ public boolean setAbsoluteEncoderOffset(double offset) @Override public double getVelocity() { - return encoder.getVelocity().getValue() * 360; + return encoder.getVelocity().getValue().in(DegreesPerSecond); } } diff --git a/swervelib/encoders/PWMDutyCycleEncoderSwerve.java b/swervelib/encoders/PWMDutyCycleEncoderSwerve.java index 1b8bb6b8..52270951 100644 --- a/swervelib/encoders/PWMDutyCycleEncoderSwerve.java +++ b/swervelib/encoders/PWMDutyCycleEncoderSwerve.java @@ -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 PWMDutyCycleEncoderSwerve(int pin) inaccurateVelocities = new Alert( "Encoders", "The PWM Duty Cycle encoder may not report accurate velocities!", - Alert.AlertType.WARNING_TRACE); + AlertType.kWarning); } diff --git a/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java index bbf9fb6c..aa88efcf 100644 --- a/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -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 @@ 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 SparkMaxAnalogEncoderSwerve(SwerveMotor motor, double maxVoltage) 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 void clearStickyFaults() @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); + } } /** diff --git a/swervelib/encoders/SparkMaxEncoderSwerve.java b/swervelib/encoders/SparkMaxEncoderSwerve.java index 6bd68dad..b01a39f1 100644 --- a/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -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 SparkMaxEncoderSwerve(SwerveMotor motor, int conversionFactor) 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 void clearStickyFaults() @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 Object getAbsoluteEncoder() @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; } diff --git a/swervelib/encoders/TalonSRXEncoderSwerve.java b/swervelib/encoders/TalonSRXEncoderSwerve.java new file mode 100644 index 00000000..d2449ed7 --- /dev/null +++ b/swervelib/encoders/TalonSRXEncoderSwerve.java @@ -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; + } + +} diff --git a/swervelib/imu/NavXSwerve.java b/swervelib/imu/NavXSwerve.java index 2d6454ef..dd2797d6 100644 --- a/swervelib/imu/NavXSwerve.java +++ b/swervelib/imu/NavXSwerve.java @@ -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 NavXSwerve(SerialPort.Port port) /* 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 NavXSwerve(SerialPort.Port port) } } - /** - * 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 void setOffset(Rotation3d offset) public void setInverted(boolean invertIMU) { invertedIMU = invertIMU; + setOffset(getRawRotation3d()); } /** diff --git a/swervelib/imu/Pigeon2Swerve.java b/swervelib/imu/Pigeon2Swerve.java index 574da147..fc0a113c 100644 --- a/swervelib/imu/Pigeon2Swerve.java +++ b/swervelib/imu/Pigeon2Swerve.java @@ -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> xAcc; + /** + * Y Accelleration supplier. + */ + private Supplier> yAcc; + /** + * Z Acceleration supplier. + */ + private Supplier> zAcc; + /** * Generate the SwerveIMU for {@link Pigeon2}. * @@ -46,6 +63,9 @@ public Pigeon2Swerve(int canid, String canbus) { 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 Rotation3d getRotation3d() 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 Rotation3d getRotation3d() @Override public Optional getAccel() { - // TODO: Switch to suppliers. - StatusSignal xAcc = imu.getAccelerationX(); - StatusSignal yAcc = imu.getAccelerationY(); - StatusSignal zAcc = imu.getAccelerationZ(); - - 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)); + // TODO: Implement later. + + 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); } /** diff --git a/swervelib/math/IMULinearMovingAverageFilter.java b/swervelib/math/IMULinearMovingAverageFilter.java index 09509b75..9ac08efb 100644 --- a/swervelib/math/IMULinearMovingAverageFilter.java +++ b/swervelib/math/IMULinearMovingAverageFilter.java @@ -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 diff --git a/swervelib/math/SwerveMath.java b/swervelib/math/SwerveMath.java index b77838c0..eea10e40 100644 --- a/swervelib/math/SwerveMath.java +++ b/swervelib/math/SwerveMath.java @@ -411,6 +411,10 @@ public static Translation2d cubeTranslation(Translation2d translation) */ 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()); } } diff --git a/swervelib/motors/SparkFlexSwerve.java b/swervelib/motors/SparkFlexSwerve.java index 400de964..e9f4b846 100644 --- a/swervelib/motors/SparkFlexSwerve.java +++ b/swervelib/motors/SparkFlexSwerve.java @@ -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 velocity; + /** + * Supplier for the position of the motor controller. + */ + private Supplier 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 SparkFlexSwerve(CANSparkFlex motor, boolean isDriveMotor) 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 @@ private void configureSparkFlex(Supplier config) { 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 @@ private void configureSparkFlex(Supplier config) @Override public void setVoltageCompensation(double nominalVoltage) { - configureSparkFlex(() -> motor.enableVoltageCompensation(nominalVoltage)); + cfg.voltageCompensation(nominalVoltage); + cfgUpdated = true; } /** @@ -134,7 +183,9 @@ public void setVoltageCompensation(double nominalVoltage) @Override public void setCurrentLimit(int currentLimit) { - configureSparkFlex(() -> motor.setSmartCurrentLimit(currentLimit)); + + cfg.smartCurrentLimit(currentLimit); + cfgUpdated = true; } /** @@ -145,8 +196,9 @@ public void setCurrentLimit(int currentLimit) @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 Object getMotor() 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 boolean isAttachedAbsoluteEncoder() @Override public void factoryDefaults() { - if (!factoryDefaultOccurred) - { - configureSparkFlex(motor::restoreFactoryDefaults); - factoryDefaultOccurred = true; - } + // Do nothing } /** @@ -205,12 +268,20 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) 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 SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) @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 void configureIntegratedEncoder(double positionConversionFactor) @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 void configurePIDF(PIDFConfig config) @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 void configureCANStatusFrames( @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 void setMotorBrake(boolean isBrakeMode) @Override public void setInverted(boolean inverted) { - configureSparkFlex(() -> { - motor.setInverted(inverted); - return motor.getLastError(); - }); + cfg.inverted(inverted); + cfgUpdated = true; } /** @@ -343,13 +420,8 @@ public void setInverted(boolean inverted) @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 void set(double percentOutput) @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 double getAppliedOutput() @Override public double getVelocity() { - return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity(); + return velocity.get(); } /** @@ -465,7 +540,7 @@ public double getVelocity() @Override public double getPosition() { - return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getAbsolutePosition(); + return position.get(); } /** @@ -482,22 +557,4 @@ public void setPosition(double 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 - } } diff --git a/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/swervelib/motors/SparkMaxBrushedMotorSwerve.java index f384352f..0502da4a 100644 --- a/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -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 encoder = Optional.empty(); /** * Closed-loop PID controller. */ - public SparkPIDController pid; + public SparkClosedLoopController pid; + /** + * Supplier for the velocity of the motor controller. + */ + private Supplier velocity; + /** + * Supplier for the position of the motor controller. + */ + private Supplier 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 SparkMaxBrushedMotorSwerve(CANSparkMax motor, boolean isDriveMotor, Type 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 @@ private void configureSparkMax(Supplier config) { 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 @@ private void configureSparkMax(Supplier config) @Override public void setVoltageCompensation(double nominalVoltage) { - configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage)); + cfg.voltageCompensation(nominalVoltage); + cfgUpdated = true; } /** @@ -171,7 +239,8 @@ public void setVoltageCompensation(double nominalVoltage) @Override public void setCurrentLimit(int currentLimit) { - configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit)); + cfg.smartCurrentLimit(currentLimit); + cfgUpdated = true; } /** @@ -182,8 +251,9 @@ public void setCurrentLimit(int currentLimit) @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 Object getMotor() 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 boolean isAttachedAbsoluteEncoder() @Override public void factoryDefaults() { - if (!factoryDefaultOccurred) - { - configureSparkMax(motor::restoreFactoryDefaults); - factoryDefaultOccurred = true; - } + // Do nothing } /** @@ -242,13 +323,32 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) 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 SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) @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 void configureIntegratedEncoder(double positionConversionFactor) @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 void configurePIDF(PIDFConfig config) @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 void configureCANStatusFrames( @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 void setMotorBrake(boolean isBrakeMode) @Override public void setInverted(boolean inverted) { - configureSparkMax(() -> { - motor.setInverted(inverted); - return motor.getLastError(); - }); + cfg.inverted(inverted); + cfgUpdated = true; } /** @@ -365,7 +499,8 @@ public void setInverted(boolean inverted) @Override public void burnFlash() { - configureSparkMax(() -> motor.burnFlash()); + motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + cfgUpdated = false; } /** @@ -388,16 +523,37 @@ public void set(double percentOutput) @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 double getAppliedOutput() @Override public double getVelocity() { - return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity(); + return velocity.get(); } /** @@ -465,7 +621,7 @@ public double getVelocity() @Override public double getPosition() { - return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getPosition(); + return position.get(); } /** @@ -478,7 +634,28 @@ public void setPosition(double position) { 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, + } } diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java index 14795716..b36da814 100644 --- a/swervelib/motors/SparkMaxSwerve.java +++ b/swervelib/motors/SparkMaxSwerve.java @@ -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 velocity; + private Supplier velocity; /** * Supplier for the position of the motor controller. */ - private Supplier position; + private Supplier 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 @@ private void configureSparkMax(Supplier config) { 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 @@ private void configureSparkMax(Supplier config) @Override public void setVoltageCompensation(double nominalVoltage) { - configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage)); + cfg.voltageCompensation(nominalVoltage); + cfgUpdated = true; } /** @@ -126,7 +170,9 @@ public void setVoltageCompensation(double nominalVoltage) @Override public void setCurrentLimit(int currentLimit) { - configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit)); + cfg.smartCurrentLimit(currentLimit); + cfgUpdated = true; + } /** @@ -137,8 +183,10 @@ public void setCurrentLimit(int currentLimit) @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 Object getMotor() 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 boolean isAttachedAbsoluteEncoder() @Override public void factoryDefaults() { - if (!factoryDefaultOccurred) - { - configureSparkMax(motor::restoreFactoryDefaults); - factoryDefaultOccurred = true; - } + // Do nothing } /** @@ -197,17 +256,23 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) 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 SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) @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,54 +314,55 @@ public void configureIntegratedEncoder(double positionConversionFactor) // 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 // 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. + // 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) { - 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 @@ else if (absoluteEncoder.getAbsoluteEncoder() instanceof SparkAnalogSensor) @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 void configurePIDF(PIDFConfig config) @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 void configureCANStatusFrames( @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 void setMotorBrake(boolean isBrakeMode) @Override public void setInverted(boolean inverted) { - configureSparkMax(() -> { - motor.setInverted(inverted); - return motor.getLastError(); - }); + cfg.inverted(inverted); + cfgUpdated = true; } /** @@ -371,13 +427,8 @@ public void setInverted(boolean inverted) @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 void set(double percentOutput) @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 void setPosition(double position) 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 - } } diff --git a/swervelib/motors/SwerveMotor.java b/swervelib/motors/SwerveMotor.java index 22b104d4..06940c6b 100644 --- a/swervelib/motors/SwerveMotor.java +++ b/swervelib/motors/SwerveMotor.java @@ -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.
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. * diff --git a/swervelib/motors/TalonFXSwerve.java b/swervelib/motors/TalonFXSwerve.java index 233a868f..73393ce8 100644 --- a/swervelib/motors/TalonFXSwerve.java +++ b/swervelib/motors/TalonFXSwerve.java @@ -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 TalonFXSwerve(TalonFX motor, boolean isDriveMotor) * @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 TalonFXSwerve(int id, String canbus, boolean isDriveMotor) * * @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 void configureIntegratedEncoder(double positionConversionFactor) 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 void setMotorBrake(boolean isBrakeMode) 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 void setReference(double setpoint, double feedforward, double position) @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 double getAppliedOutput() @Override public double getVelocity() { - return motor.getVelocity().getValue(); + return motor.getVelocity().getValue().magnitude(); } /** @@ -401,7 +358,7 @@ public double getVelocity() @Override public double getPosition() { - return motor.getPosition().getValue(); + return motor.getPosition().getValue().magnitude(); } /** @@ -414,8 +371,7 @@ public void setPosition(double position) { 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 void setCurrentLimit(int currentLimit) { cfg.refresh(configuration.CurrentLimits); cfg.apply( - configuration.CurrentLimits.withStatorCurrentLimit(currentLimit) - .withStatorCurrentLimitEnable(true)); + configuration.CurrentLimits.withSupplyCurrentLimit(currentLimit) + .withSupplyCurrentLimitEnable(true)); } /** @@ -468,6 +424,21 @@ public Object getMotor() 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. * diff --git a/swervelib/motors/TalonSRXSwerve.java b/swervelib/motors/TalonSRXSwerve.java index 98cf6e7c..5c8f690f 100644 --- a/swervelib/motors/TalonSRXSwerve.java +++ b/swervelib/motors/TalonSRXSwerve.java @@ -7,6 +7,8 @@ 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 TalonSRXSwerve(WPI_TalonSRX motor, boolean isDriveMotor) * * @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 double getPosition() } else { var pos = motor.getSelectedSensorPosition() * positionConversionFactor; - pos %= 360; - if (pos < 360) - { - pos += 360; - } return pos; } } @@ -421,6 +421,17 @@ public void setLoopRampRate(double rampRate) 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 Object getMotor() 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. * diff --git a/swervelib/parser/Cache.java b/swervelib/parser/Cache.java index 830a08c0..830a2731 100644 --- a/swervelib/parser/Cache.java +++ b/swervelib/parser/Cache.java @@ -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 Cache updateValidityPeriod(long validityPeriod) */ public T getValue() { - if (isStale()) + if (isStale() || RobotBase.isSimulation()) { update(); } diff --git a/swervelib/parser/SwerveControllerConfiguration.java b/swervelib/parser/SwerveControllerConfiguration.java index 7f1d9671..9fd438eb 100644 --- a/swervelib/parser/SwerveControllerConfiguration.java +++ b/swervelib/parser/SwerveControllerConfiguration.java @@ -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; diff --git a/swervelib/parser/SwerveDriveConfiguration.java b/swervelib/parser/SwerveDriveConfiguration.java index 347b187d..6c62581a 100644 --- a/swervelib/parser/SwerveDriveConfiguration.java +++ b/swervelib/parser/SwerveDriveConfiguration.java @@ -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 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 double getDriveBaseRadiusMeters() //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 getGyroSim() + { + if (imu instanceof Pigeon2Swerve) + { + return GyroSimulation.getPigeon2(); + } else if (imu instanceof NavXSwerve) + { + return GyroSimulation.getNav2X(); + } + return GyroSimulation.getGeneric(); + } + } diff --git a/swervelib/parser/SwerveModuleConfiguration.java b/swervelib/parser/SwerveModuleConfiguration.java index 24d06257..8996f448 100644 --- a/swervelib/parser/SwerveModuleConfiguration.java +++ b/swervelib/parser/SwerveModuleConfiguration.java @@ -3,7 +3,7 @@ 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 SwerveModuleConfiguration( public SwerveModuleConfiguration( SwerveMotor driveMotor, SwerveMotor angleMotor, - MotorConfigDouble conversionFactors, + ConversionFactorsJson conversionFactors, SwerveAbsoluteEncoder absoluteEncoder, double angleOffset, double xMeters, diff --git a/swervelib/parser/SwerveModulePhysicalCharacteristics.java b/swervelib/parser/SwerveModulePhysicalCharacteristics.java index 99360ce1..a626bbb8 100644 --- a/swervelib/parser/SwerveModulePhysicalCharacteristics.java +++ b/swervelib/parser/SwerveModulePhysicalCharacteristics.java @@ -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 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 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 SwerveModulePhysicalCharacteristics( * power and power loss). */ public SwerveModulePhysicalCharacteristics( - MotorConfigDouble conversionFactors, + ConversionFactorsJson conversionFactors, double driveMotorRampRate, double angleMotorRampRate) { @@ -101,6 +125,10 @@ public SwerveModulePhysicalCharacteristics( 40, 20, driveMotorRampRate, - angleMotorRampRate); + angleMotorRampRate, + 0.2, + 0.3, + 0.03, + 50); } } diff --git a/swervelib/parser/SwerveParser.java b/swervelib/parser/SwerveParser.java index 573fdd87..afa53544 100644 --- a/swervelib/parser/SwerveParser.java +++ b/swervelib/parser/SwerveParser.java @@ -2,7 +2,7 @@ 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 @@ private void checkDirectory(File directory) */ 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 SwerveDrive createSwerveDrive(double maxSpeed) */ 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)}. - * @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 + * @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) + public SwerveDrive createSwerveDrive(double maxSpeed, Pose2d initialPose) { SwerveModuleConfiguration[] moduleConfigurations = new SwerveModuleConfiguration[moduleJsons.length]; @@ -217,11 +186,12 @@ public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, do moduleConfigurations, swerveDriveJson.imu.createIMU(), swerveDriveJson.invertedIMU, - driveFeedforward, physicalPropertiesJson.createPhysicalProperties()); return new SwerveDrive( swerveDriveConfiguration, - controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed), maxSpeed); + controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed), + maxSpeed, + initialPose); } } diff --git a/swervelib/parser/json/DeviceJson.java b/swervelib/parser/json/DeviceJson.java index 7ce6fc3d..71a11a44 100644 --- a/swervelib/parser/json/DeviceJson.java +++ b/swervelib/parser/json/DeviceJson.java @@ -4,11 +4,10 @@ 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.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.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 SwerveAbsoluteEncoder createEncoder(SwerveMotor motor) 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 SwerveIMU createIMU() 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 SwerveIMU createIMU() ".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 SwerveMotor createMotor(boolean isDriveMotor) } 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."); } diff --git a/swervelib/parser/json/ModuleJson.java b/swervelib/parser/json/ModuleJson.java index 2fa83c89..f26b2e84 100644 --- a/swervelib/parser/json/ModuleJson.java +++ b/swervelib/parser/json/ModuleJson.java @@ -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 - *

- * 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 SwerveModuleConfiguration createModuleConfiguration( 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 SwerveModuleConfiguration createModuleConfiguration( "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 SwerveModuleConfiguration createModuleConfiguration( return new SwerveModuleConfiguration( drive.createMotor(true), angleMotor, - conversionFactor, + conversionFactors, absEncoder, absoluteEncoderOffset, Units.inchesToMeters(Math.round(location.x) == 0 ? location.front : location.x), diff --git a/swervelib/parser/json/PhysicalPropertiesJson.java b/swervelib/parser/json/PhysicalPropertiesJson.java index 60be9d25..eaab95d6 100644 --- a/swervelib/parser/json/PhysicalPropertiesJson.java +++ b/swervelib/parser/json/PhysicalPropertiesJson.java @@ -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 @@ 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. + */ + @Deprecated(since = "2025", forRemoval = true) + public MotorConfigDouble conversionFactor = new MotorConfigDouble(); + /** + * Minimum voltage to spin the module or wheel. */ - public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); + 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()) - { - conversionFactor.angle = conversionFactors.angle.calculate(); - } - - if (!conversionFactors.isDriveEmpty()) + if (conversionFactor.drive != 0 && conversionFactor.angle != 0 && conversionFactors.isDriveEmpty() && + conversionFactors.isAngleEmpty()) { - 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)); } } diff --git a/swervelib/parser/json/modules/AngleConversionFactorsJson.java b/swervelib/parser/json/modules/AngleConversionFactorsJson.java index a7c8613a..5a3e10f9 100644 --- a/swervelib/parser/json/modules/AngleConversionFactorsJson.java +++ b/swervelib/parser/json/modules/AngleConversionFactorsJson.java @@ -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); diff --git a/swervelib/parser/json/modules/ConversionFactorsJson.java b/swervelib/parser/json/modules/ConversionFactorsJson.java index 85ff25f2..546ad65a 100644 --- a/swervelib/parser/json/modules/ConversionFactorsJson.java +++ b/swervelib/parser/json/modules/ConversionFactorsJson.java @@ -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 boolean isDriveEmpty() */ 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); } } diff --git a/swervelib/parser/json/modules/DriveConversionFactorsJson.java b/swervelib/parser/json/modules/DriveConversionFactorsJson.java index d792d36d..937b78a1 100644 --- a/swervelib/parser/json/modules/DriveConversionFactorsJson.java +++ b/swervelib/parser/json/modules/DriveConversionFactorsJson.java @@ -2,8 +2,6 @@ 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); diff --git a/swervelib/simulation/SwerveIMUSimulation.java b/swervelib/simulation/SwerveIMUSimulation.java index b3f2b62d..aa92c7e8 100644 --- a/swervelib/simulation/SwerveIMUSimulation.java +++ b/swervelib/simulation/SwerveIMUSimulation.java @@ -6,9 +6,9 @@ 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 @@ 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 SwerveIMUSimulation() */ public Rotation2d getYaw() { - return new Rotation2d(angle); + return gyroSimulation.getGyroReading(); } /** @@ -76,7 +65,7 @@ public Rotation2d getRoll() */ public Rotation3d getGyroRotation3d() { - return new Rotation3d(0, 0, angle); + return new Rotation3d(0, 0, getYaw().getRadians()); } /** @@ -104,9 +93,6 @@ public void updateOdometry( 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 void updateOdometry( */ public void setAngle(double angle) { - this.angle = angle; + this.gyroSimulation.setRotation(Rotation2d.fromRadians(angle)); } } diff --git a/swervelib/simulation/SwerveModuleSimulation.java b/swervelib/simulation/SwerveModuleSimulation.java index 2fb06f8f..76b15ec0 100644 --- a/swervelib/simulation/SwerveModuleSimulation.java +++ b/swervelib/simulation/SwerveModuleSimulation.java @@ -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. - */ - 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. + * Configure the maple sim module + * + * @param simModule the {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} object for simulation */ - 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 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 void updateStateAndPosition(SwerveModuleState desiredState) */ public SwerveModulePosition getPosition() { - - return new SwerveModulePosition(fakePos, state.angle); + return mapleSimModule.getModulePosition(); } /** @@ -86,6 +58,12 @@ public SwerveModulePosition getPosition() */ public SwerveModuleState getState() { + if (mapleSimModule == null) + { + return new SwerveModuleState(); + } + SwerveModuleState state = mapleSimModule.getMeasuredState(); + state.angle = state.angle.minus(Rotation2d.kZero); return state; } } diff --git a/swervelib/telemetry/SwerveDriveTelemetry.java b/swervelib/telemetry/SwerveDriveTelemetry.java index 3c90fc6f..07d984bc 100644 --- a/swervelib/telemetry/SwerveDriveTelemetry.java +++ b/swervelib/telemetry/SwerveDriveTelemetry.java @@ -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); + /** + * Measured swerve module states object. + */ + public static SwerveModuleState[] measuredStatesObj = new SwerveModuleState[4]; + /** + * Desired swerve module states object + */ + public static SwerveModuleState[] desiredStatesObj = new SwerveModuleState[4]; + /** + * The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the + * chassis speeds properties. + */ + public static ChassisSpeeds measuredChassisSpeedsObj = new ChassisSpeeds(); + /** + * Describes the desired forward, sideways and angular velocity of the robot. + */ + public static ChassisSpeeds desiredChassisSpeedsObj = new ChassisSpeeds(); + /** + * The robot's current rotation based on odometry or gyro readings + */ + public static Rotation2d robotRotationObj = new Rotation2d(); /** * The current telemetry verbosity level. */ - public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE; + public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE; /** * State of simulation of the Robot, used to optimize retrieval. */ - public static boolean isSimulation = RobotBase.isSimulation(); + public static boolean isSimulation = RobotBase.isSimulation(); /** * The number of swerve modules */ - public static int moduleCount; + public static int moduleCount; /** * The Locations of the swerve drive wheels. */ - public static double[] wheelLocations; + public static double[] wheelLocations; /** * An array of rotation and velocity values describing the measured state of each swerve module */ - public static double[] measuredStates; + 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 double[] desiredStates; /** * The robot's current rotation based on odometry or gyro readings */ - public static double robotRotation = 0; + 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 measuredStatesStruct + = NetworkTableInstance.getDefault() + .getStructArrayTopic( + "swerve/advantagescope/currentStates", + SwerveModuleState.struct) + .publish(); + /** + * Struct publisher for AdvantageScope swerve widgets. + */ + private static StructArrayPublisher desiredStatesStruct + = NetworkTableInstance.getDefault() + .getStructArrayTopic( + "swerve/advantagescope/desiredStates", + SwerveModuleState.struct) + .publish(); + /** + * Measured {@link ChassisSpeeds} for NT4 AdvantageScope swerve widgets. + */ + private static StructPublisher measuredChassisSpeedsStruct + = NetworkTableInstance.getDefault() + .getStructTopic( + "swerve/advantagescope/measuredChassisSpeeds", + ChassisSpeeds.struct) + .publish(); + /** + * Desired {@link ChassisSpeeds} for NT4 AdvantageScope swerve widgets. + */ + private static StructPublisher desiredChassisSpeedsStruct + = NetworkTableInstance.getDefault() + .getStructTopic( + "swerve/advantagescope/desiredChassisSpeeds", + ChassisSpeeds.struct) + .publish(); + /** + * Robot {@link Rotation2d} for AdvantageScope swerve widgets. + */ + private static StructPublisher 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 static void updateData() 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); } /**