diff --git a/src/main/deploy/apriltags/2023-chargedup.json b/src/main/deploy/apriltags/2023-chargedup.json deleted file mode 100644 index 368081b9..00000000 --- a/src/main/deploy/apriltags/2023-chargedup.json +++ /dev/null @@ -1,152 +0,0 @@ -{ - "tags": [ - { - "ID": 1, - "pose": { - "translation": { - "x": 15.513558, - "y": 1.071626, - "z": 0.462788 - }, - "rotation": { - "quaternion": { - "W": 0.0, - "X": 0.0, - "Y": 0.0, - "Z": 1.0 - } - } - } - }, - { - "ID": 2, - "pose": { - "translation": { - "x": 15.513558, - "y": 2.748026, - "z": 0.462788 - }, - "rotation": { - "quaternion": { - "W": 0.0, - "X": 0.0, - "Y": 0.0, - "Z": 1.0 - } - } - } - }, - { - "ID": 3, - "pose": { - "translation": { - "x": 15.513558, - "y": 4.424426, - "z": 0.462788 - }, - "rotation": { - "quaternion": { - "W": 0.0, - "X": 0.0, - "Y": 0.0, - "Z": 1.0 - } - } - } - }, - { - "ID": 4, - "pose": { - "translation": { - "x": 16.178784, - "y": 6.749796, - "z": 0.695452 - }, - "rotation": { - "quaternion": { - "W": 0.0, - "X": 0.0, - "Y": 0.0, - "Z": 1.0 - } - } - } - }, - { - "ID": 5, - "pose": { - "translation": { - "x": 0.36195, - "y": 6.749796, - "z": 0.695452 - }, - "rotation": { - "quaternion": { - "W": 1.0, - "X": 0.0, - "Y": 0.0, - "Z": 0.0 - } - } - } - }, - { - "ID": 6, - "pose": { - "translation": { - "x": 1.02743, - "y": 4.424426, - "z": 0.462788 - }, - "rotation": { - "quaternion": { - "W": 1.0, - "X": 0.0, - "Y": 0.0, - "Z": 0.0 - } - } - } - }, - { - "ID": 7, - "pose": { - "translation": { - "x": 1.02743, - "y": 2.748026, - "z": 0.462788 - }, - "rotation": { - "quaternion": { - "W": 1.0, - "X": 0.0, - "Y": 0.0, - "Z": 0.0 - } - } - } - }, - { - "ID": 8, - "pose": { - "translation": { - "x": 1.02743, - "y": 1.071626, - "z": 0.462788 - }, - "rotation": { - "quaternion": { - "W": 1.0, - "X": 0.0, - "Y": 0.0, - "Z": 0.0 - } - } - } - } - ], - "field": { - "length": 16.54175, - "width": 8.0137 - } -} \ No newline at end of file diff --git a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/physicalproperties.json b/src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/physicalproperties.json deleted file mode 100644 index dd574ad8..00000000 --- a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/physicalproperties.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - "wheelDiameter": 3, - "gearRatio": { - "drive": 4.714285714285714, - "angle": 1 - }, - "currentLimit": { - "drive": 40, - "angle": 20 - }, - "rampRate": { - "drive": 0.25, - "angle": 0.25 - }, - "encoderPulsePerRotation": { - "drive": 1, - "angle": 1 - }, - "wheelGripCoefficientOfFriction": 1.19, - "angleMotorFreeSpeedRPM": 5676 -} \ No newline at end of file diff --git a/src/main/deploy/swerve/falcon/modules/physicalproperties.json b/src/main/deploy/swerve/falcon/modules/physicalproperties.json index d09b181b..8d11c027 100644 --- a/src/main/deploy/swerve/falcon/modules/physicalproperties.json +++ b/src/main/deploy/swerve/falcon/modules/physicalproperties.json @@ -1,7 +1,7 @@ { "conversionFactor": { - "angle": 0.01373291015625, - "drive": 1.914649238933429E-5 + "angle": 28.125, + "drive": 0.047286787200699704 }, "currentLimit": { "drive": 40, diff --git a/src/main/deploy/swerve/falcon/modules/pidfproperties.json b/src/main/deploy/swerve/falcon/modules/pidfproperties.json index 81bae4b0..b562c2b1 100644 --- a/src/main/deploy/swerve/falcon/modules/pidfproperties.json +++ b/src/main/deploy/swerve/falcon/modules/pidfproperties.json @@ -1,15 +1,15 @@ { "drive": { - "p": 0.15, + "p": 1, "i": 0, - "d": 2.0, + "d": 0, "f": 0, "iz": 0 }, "angle": { - "p": 0.050953, + "p": 360, "i": 0, - "d": 0.118, + "d": 0, "f": 0, "iz": 0 } diff --git a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/controllerproperties.json b/src/main/deploy/swerve/maxSwerve/controllerproperties.json similarity index 100% rename from src/main/deploy/swerve/MaxSwerve-Neo-NavX/controllerproperties.json rename to src/main/deploy/swerve/maxSwerve/controllerproperties.json diff --git a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/backleft.json b/src/main/deploy/swerve/maxSwerve/modules/backleft.json similarity index 100% rename from src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/backleft.json rename to src/main/deploy/swerve/maxSwerve/modules/backleft.json diff --git a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/backright.json b/src/main/deploy/swerve/maxSwerve/modules/backright.json similarity index 100% rename from src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/backright.json rename to src/main/deploy/swerve/maxSwerve/modules/backright.json diff --git a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/frontleft.json b/src/main/deploy/swerve/maxSwerve/modules/frontleft.json similarity index 100% rename from src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/frontleft.json rename to src/main/deploy/swerve/maxSwerve/modules/frontleft.json diff --git a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/frontright.json b/src/main/deploy/swerve/maxSwerve/modules/frontright.json similarity index 100% rename from src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/frontright.json rename to src/main/deploy/swerve/maxSwerve/modules/frontright.json diff --git a/src/main/deploy/swerve/maxSwerve/modules/physicalproperties.json b/src/main/deploy/swerve/maxSwerve/modules/physicalproperties.json new file mode 100644 index 00000000..55a1388d --- /dev/null +++ b/src/main/deploy/swerve/maxSwerve/modules/physicalproperties.json @@ -0,0 +1,16 @@ +{ + "conversionFactor": { + "drive": 0.047286787200699704, + "angle": 360 + }, + "currentLimit": { + "drive": 40, + "angle": 20 + }, + "rampRate": { + "drive": 0.25, + "angle": 0.25 + }, + "optimalVoltage": 12, + "wheelGripCoefficientOfFriction": 1.19 +} \ No newline at end of file diff --git a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/pidfproperties.json b/src/main/deploy/swerve/maxSwerve/modules/pidfproperties.json similarity index 100% rename from src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/pidfproperties.json rename to src/main/deploy/swerve/maxSwerve/modules/pidfproperties.json diff --git a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/swervedrive.json b/src/main/deploy/swerve/maxSwerve/swervedrive.json similarity index 73% rename from src/main/deploy/swerve/MaxSwerve-Neo-NavX/swervedrive.json rename to src/main/deploy/swerve/maxSwerve/swervedrive.json index dd4d3253..b42e51e0 100644 --- a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/swervedrive.json +++ b/src/main/deploy/swerve/maxSwerve/swervedrive.json @@ -1,12 +1,10 @@ { - "maxSpeed": 14.5, - "optimalVoltage": 12, "imu": { "type": "navx_spi", "id": 0, "canbus": null }, - "invertedIMU": true, + "invertedIMU": false, "modules": [ "frontleft.json", "frontright.json", diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 91e41341..929ff4d0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -47,6 +47,6 @@ public static class OperatorConstants public static final double LEFT_X_DEADBAND = 0.01; public static final double LEFT_Y_DEADBAND = 0.01; public static final double RIGHT_X_DEADBAND = 0.01; - public static final double TURN_CONSTANT = 0.75; + public static final double TURN_CONSTANT = 6; } } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java index 1c725458..422d4477 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.networktables.BooleanSubscriber; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; @@ -27,7 +26,7 @@ public class AbsoluteDriveAdv extends Command private final SwerveSubsystem swerve; private final DoubleSupplier vX, vY; private final DoubleSupplier headingAdjust; - private boolean initRotation = false; + private boolean resetHeading = false; private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight; /** @@ -67,7 +66,7 @@ public AbsoluteDriveAdv(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplie @Override public void initialize() { - initRotation = true; + resetHeading = true; } // Called every time the scheduler runs while the command is scheduled. @@ -76,53 +75,43 @@ public void execute() { double headingX = 0; double headingY = 0; - Rotation2d newHeading = Rotation2d.fromRadians(0); // These are written to allow combinations for 45 angles // Face Away from Drivers if(lookAway.getAsBoolean()){ - headingX = 1; + headingY = -1; } // Face Right if(lookRight.getAsBoolean()){ - headingY = 1; + headingX = 1; } // Face Left if(lookLeft.getAsBoolean()){ - headingY = -1; + headingX = -1; } // Face Towards the Drivers if(lookTowards.getAsBoolean()){ - headingX = -1; - } - - //Dont overwrite a button press - if(headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0){ - newHeading = Rotation2d.fromRadians(Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()) - .plus(swerve.getHeading()); - headingX = newHeading.getSin(); - headingY = newHeading.getCos(); + headingY = 1; } - ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), - headingX, - headingY); - // Prevent Movement After Auto - if(initRotation) + if(resetHeading) { - if(headingX == 0 && headingY == 0) + if(headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { - // Get the curretHeading - Rotation2d firstLoopHeading = swerve.getHeading(); + // Get the curret Heading + Rotation2d currentHeading = swerve.getHeading(); // Set the Current Heading to the desired Heading - desiredSpeeds = swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos()); + headingX = currentHeading.getSin(); + headingY = currentHeading.getCos(); } - //Dont Init Rotation Again - initRotation = false; + //Dont reset Heading Again + resetHeading = false; } + ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY); + // Limit velocity to prevent tippy Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(), @@ -132,7 +121,13 @@ public void execute() SmartDashboard.putString("Translation", translation.toString()); // Make the robot move - swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + if(headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0){ + resetHeading = true; + swerve.drive(translation, (Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), true); + } + else{ + swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 35a38191..a8e76451 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -11,6 +11,7 @@ import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -21,6 +22,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.io.File; +import java.util.function.DoubleSupplier; import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.math.SwerveMath; @@ -104,12 +106,12 @@ public void setupPathPlanner() // Default path replanning config. See the API for the options here ), () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - var alliance = DriverStation.getAlliance(); - return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; - }, + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + var alliance = DriverStation.getAlliance(); + return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; + }, this // Reference to this subsystem to set requirements ); } @@ -135,6 +137,45 @@ public Command getAutonomousCommand(String pathName, boolean setOdomToStart) return AutoBuilder.followPath(path); } + /** + * Command to drive the robot using translative values and heading as a setpoint. + * + * @param translationX Translation in the X direction. + * @param translationY Translation in the Y direction. + * @param headingX Heading X to calculate angle of the joystick. + * @param headingY Heading Y to calculate angle of the joystick. + * @return Drive command. + */ + public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX, + DoubleSupplier headingY) + { + return run(() -> { + // Make the robot move + driveFieldOriented(getTargetSpeeds(translationX.getAsDouble(), translationY.getAsDouble(), + headingX.getAsDouble(), + headingY.getAsDouble())); + }); + } + + /** + * Command to drive the robot using translative values and heading as angular velocity. + * + * @param translationX Translation in the X direction. + * @param translationY Translation in the Y direction. + * @param angularRotationX Rotation of the robot to set + * @return Drive command. + */ + public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) + { + return run(() -> { + // Make the robot move + swerveDrive.drive(new Translation2d(translationX.getAsDouble() * maximumSpeed, translationY.getAsDouble()), + angularRotationX.getAsDouble() * swerveDrive.swerveController.config.maxAngularVelocity, + true, + false); + }); + } + /** * Construct the swerve drive. * @@ -301,7 +342,8 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headin } /** - * Get the chassis speeds based on controller input of 1 joystick and one angle. + * Get the chassis speeds based on controller input of 1 joystick and one angle. Control the robot at an offset of + * 90deg. * * @param xInput X joystick input for the robot to move in the X direction. * @param yInput Y joystick input for the robot to move in the Y direction. diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 2adc901c..5296f3f4 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -11,7 +11,6 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -67,6 +66,10 @@ public class SwerveDrive * Odometry lock to ensure thread safety. */ private final Lock odometryLock = new ReentrantLock(); + /** + * Deadband for speeds in heading correction. + */ + private final double HEADING_CORRECTION_DEADBAND = 0.01; /** * Field object. */ @@ -100,14 +103,14 @@ public class SwerveDrive * correction. */ public boolean chassisVelocityCorrection = true; - /** - * Whether heading correction PID is currently active. - */ - private boolean correctionEnabled = false; /** * Whether to correct heading when driving translationally. Set to true to enable. */ public boolean headingCorrection = false; + /** + * Whether heading correction PID is currently active. + */ + private boolean correctionEnabled = false; /** * Swerve IMU device for sensing the heading of the robot. */ @@ -120,10 +123,6 @@ public class SwerveDrive * Counter to synchronize the modules relative encoder with absolute encoder when not moving. */ private int moduleSynchronizationCounter = 0; - /** - * Deadband for speeds in heading correction. - */ - private final double HEADING_CORRECTION_DEADBAND = 0.01; /** * The last heading set in radians. */ @@ -413,10 +412,10 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent if (headingCorrection) { if (Math.abs(velocity.omegaRadiansPerSecond) < HEADING_CORRECTION_DEADBAND - && (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND - || Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND)) + && (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND + || Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND)) { - if (!correctionEnabled) + if (!correctionEnabled) { lastHeadingRadians = getYaw().getRadians(); correctionEnabled = true; diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 82b3ca92..fe3989aa 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -5,13 +5,13 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.math.SwerveMath; import swervelib.motors.SwerveMotor; import swervelib.parser.SwerveModuleConfiguration; import swervelib.simulation.SwerveModuleSimulation; +import swervelib.telemetry.Alert; import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @@ -33,6 +33,14 @@ public class SwerveModule * Absolute encoder for swerve drive. */ private final SwerveAbsoluteEncoder absoluteEncoder; + /** + * An {@link Alert} for if pushing the Absolute Encoder offset to the encoder fails. + */ + private final Alert encoderOffsetWarning; + /** + * An {@link Alert} for if there is no Absolute Encoder on the module. + */ + private final Alert noEncoderWarning; /** * Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right. */ @@ -129,6 +137,15 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat } lastState = getState(); + + noEncoderWarning = new Alert("Motors", + "There is no Absolute Encoder on module #" + + moduleNumber, + Alert.AlertType.WARNING); + encoderOffsetWarning = new Alert("Motors", + "Pushing the Absolute Encoder offset to the encoder failed on module #" + + moduleNumber, + Alert.AlertType.WARNING); } /** @@ -399,12 +416,11 @@ public void pushOffsetsToControllers() angleOffset = 0; } else { - DriverStation.reportWarning( - "Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, false); + encoderOffsetWarning.set(true); } } else { - DriverStation.reportWarning("There is no Absolute Encoder on module #" + moduleNumber, false); + noEncoderWarning.set(true); } } diff --git a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java index 1b830b1b..bcf4df95 100644 --- a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java @@ -1,8 +1,8 @@ package swervelib.encoders; import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; +import swervelib.telemetry.Alert; /** * Swerve Absolute Encoder for Thrifty Encoders and other analog encoders. @@ -19,6 +19,14 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder * Inversion state of the encoder. */ private boolean inverted = false; + /** + * An {@link Alert} for if the absolute encoder offset cannot be set. + */ + private Alert cannotSetOffset; + /** + * An {@link Alert} detailing how the analog absolute encoder may not report accurate velocities. + */ + private Alert inaccurateVelocities; /** * Construct the Thrifty Encoder as a Swerve Absolute Encoder. @@ -28,6 +36,14 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder public AnalogAbsoluteEncoderSwerve(AnalogInput encoder) { this.encoder = encoder; + cannotSetOffset = new Alert( + "Encoders", + "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), + Alert.AlertType.WARNING); + inaccurateVelocities = new Alert( + "Encoders", + "The Analog Absolute encoder may not report accurate velocities!", + Alert.AlertType.WARNING_TRACE); } /** @@ -101,8 +117,7 @@ public Object getAbsoluteEncoder() public boolean setAbsoluteEncoderOffset(double offset) { //Do Nothing - DriverStation.reportWarning( - "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), false); + cannotSetOffset.set(true); return false; } @@ -114,7 +129,7 @@ public boolean setAbsoluteEncoderOffset(double offset) @Override public double getVelocity() { - DriverStation.reportWarning("The Analog Absolute encoder may not report accurate velocities!", true); + inaccurateVelocities.set(true); return encoder.getValue(); } } diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java index f7ad6f4b..011560c7 100644 --- a/src/main/java/swervelib/encoders/CANCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CANCoderSwerve.java @@ -9,7 +9,7 @@ import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.ctre.phoenix6.signals.MagnetHealthValue; import com.ctre.phoenix6.signals.SensorDirectionValue; -import edu.wpi.first.wpilibj.DriverStation; +import swervelib.telemetry.Alert; /** * Swerve Absolute Encoder for CTRE CANCoders. @@ -20,7 +20,23 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder /** * CANCoder with WPILib sendable and support. */ - public CANcoder encoder; + public CANcoder encoder; + /** + * An {@link Alert} for if the CANCoder magnet field is less than ideal. + */ + private Alert magnetFieldLessThanIdeal; + /** + * An {@link Alert} for if the CANCoder reading is faulty. + */ + private Alert readingFaulty; + /** + * An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored. + */ + private Alert readingIgnored; + /** + * An {@link Alert} for if the absolute encoder offset cannot be set. + */ + private Alert cannotSetOffset; /** * Initialize the CANCoder on the standard CANBus. @@ -41,6 +57,24 @@ public CANCoderSwerve(int id) public CANCoderSwerve(int id, String canbus) { encoder = new CANcoder(id, canbus); + magnetFieldLessThanIdeal = new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.", + Alert.AlertType.WARNING); + readingFaulty = new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " reading was faulty.", + Alert.AlertType.WARNING); + readingIgnored = new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.", + Alert.AlertType.WARNING); + cannotSetOffset = new Alert( + "Encoders", + "Failure to set CANCoder " + + encoder.getDeviceID() + + " Absolute Encoder Offset", + Alert.AlertType.WARNING); } /** @@ -89,17 +123,17 @@ public double getAbsolutePosition() readingError = false; MagnetHealthValue strength = encoder.getMagnetHealth().getValue(); - if (strength != MagnetHealthValue.Magnet_Green) - { - DriverStation.reportWarning( - "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.\n", false); - } + magnetFieldLessThanIdeal.set(strength != MagnetHealthValue.Magnet_Green); if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red) { readingError = true; - DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty.\n", false); -// return 0; + readingFaulty.set(true); + return 0; + } else + { + readingFaulty.set(false); } + StatusSignal angle = encoder.getAbsolutePosition().refresh(); // Taken from democat's library. @@ -115,7 +149,10 @@ public double getAbsolutePosition() if (angle.getStatus() != StatusCode.OK) { readingError = true; - DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.\n", false); + readingIgnored.set(true); + } else + { + readingIgnored.set(false); } return angle.getValue() * 360; @@ -149,12 +186,17 @@ public boolean setAbsoluteEncoderOffset(double offset) return false; } error = cfg.apply(magCfg.withMagnetOffset(offset / 360)); + cannotSetOffset.setText( + "Failure to set CANCoder " + + encoder.getDeviceID() + + " Absolute Encoder Offset Error: " + + error); if (error == StatusCode.OK) { + cannotSetOffset.set(false); return true; } - DriverStation.reportWarning( - "Failure to set CANCoder " + encoder.getDeviceID() + " Absolute Encoder Offset Error: " + error, false); + cannotSetOffset.set(true); return false; } diff --git a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java index 9ab00987..1c6b7b13 100644 --- a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java @@ -25,7 +25,7 @@ public CanAndCoderSwerve(int canid) /** * Reset the encoder to factory defaults. - * + *

* This will not clear the stored zero offset. */ @Override diff --git a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java index 0361aec8..24e15d66 100644 --- a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java @@ -1,7 +1,7 @@ package swervelib.encoders; -import edu.wpi.first.wpilibj.DriverStation; 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 @@ -22,6 +22,10 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder * Inversion state. */ private boolean isInverted; + /** + * An {@link Alert} for if the encoder cannot report accurate velocities. + */ + private Alert inaccurateVelocities; /** * Constructor for the PWM duty cycle encoder. @@ -31,6 +35,11 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder public PWMDutyCycleEncoderSwerve(int pin) { encoder = new DutyCycleEncoder(pin); + inaccurateVelocities = new Alert( + "Encoders", + "The PWM Duty Cycle encoder may not report accurate velocities!", + Alert.AlertType.WARNING_TRACE); + } /** @@ -74,7 +83,7 @@ public Object getAbsoluteEncoder() @Override public double getVelocity() { - DriverStation.reportWarning("The PWM Duty Cycle encoder may not report accurate velocities!", true); + inaccurateVelocities.set(true); return encoder.get(); } diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java index 0508bca8..30247879 100644 --- a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -4,9 +4,9 @@ import com.revrobotics.REVLibError; import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkAnalogSensor.Mode; -import edu.wpi.first.wpilibj.DriverStation; import java.util.function.Supplier; import swervelib.motors.SwerveMotor; +import swervelib.telemetry.Alert; /** * SparkMax absolute encoder, attached through the data port analog pin. @@ -17,7 +17,16 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder /** * 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; + /** + * An {@link Alert} for if the absolute encoder does not support integrated offsets. + */ + private Alert doesNotSupportIntegratedOffsets; + /** * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link CANSparkMax} motor data @@ -34,6 +43,15 @@ public SparkMaxAnalogEncoderSwerve(SwerveMotor motor) { throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); } + failureConfiguring = new Alert( + "Encoders", + "Failure configuring SparkMax Analog Encoder", + Alert.AlertType.WARNING_TRACE); + doesNotSupportIntegratedOffsets = new Alert( + "Encoders", + "SparkMax Analog Sensors do not support integrated offsets", + Alert.AlertType.WARNING_TRACE); + } /** @@ -50,7 +68,7 @@ private void configureSparkMax(Supplier config) return; } } - DriverStation.reportWarning("Failure configuring encoder", true); + failureConfiguring.set(true); } /** @@ -113,7 +131,7 @@ public Object getAbsoluteEncoder() @Override public boolean setAbsoluteEncoderOffset(double offset) { - DriverStation.reportWarning("SparkMax Analog Sensor's do not support integrated offsets", true); + doesNotSupportIntegratedOffsets.set(true); return false; } diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java index f95d36d2..dd81adae 100644 --- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -4,9 +4,9 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; import com.revrobotics.SparkAbsoluteEncoder.Type; -import edu.wpi.first.wpilibj.DriverStation; import java.util.function.Supplier; import swervelib.motors.SwerveMotor; +import swervelib.telemetry.Alert; /** * SparkMax absolute encoder, attached through the data port. @@ -17,7 +17,15 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder /** * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax. */ - public AbsoluteEncoder encoder; + public AbsoluteEncoder encoder; + /** + * An {@link Alert} for if there is a failure configuring the encoder. + */ + private Alert failureConfiguring; + /** + * An {@link Alert} for if there is a failure configuring the encoder offset. + */ + private Alert offsetFailure; /** * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link CANSparkMax} motor. @@ -36,6 +44,14 @@ public SparkMaxEncoderSwerve(SwerveMotor motor, int conversionFactor) { throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); } + failureConfiguring = new Alert( + "Encoders", + "Failure configuring SparkMax Analog Encoder", + Alert.AlertType.WARNING_TRACE); + offsetFailure = new Alert( + "Encoders", + "Failure to set Absolute Encoder Offset", + Alert.AlertType.WARNING_TRACE); } /** @@ -52,7 +68,7 @@ private void configureSparkMax(Supplier config) return; } } - DriverStation.reportWarning("Failure configuring encoder", true); + failureConfiguring.set(true); } /** @@ -124,7 +140,8 @@ public boolean setAbsoluteEncoderOffset(double offset) return true; } } - DriverStation.reportWarning("Failure to set Absolute Encoder Offset Error: " + error, false); + offsetFailure.setText("Failure to set Absolute Encoder Offset Error: " + error); + offsetFailure.set(true); return false; } diff --git a/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java b/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java index a01e4c47..c992b81a 100644 --- a/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java +++ b/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java @@ -56,6 +56,7 @@ public abstract class SwerveAbsoluteEncoder /** * Get the velocity in degrees/sec. + * * @return velocity in degrees/sec. */ public abstract double getVelocity(); diff --git a/src/main/java/swervelib/imu/ADIS16470Swerve.java b/src/main/java/swervelib/imu/ADIS16470Swerve.java index d0da6ff8..4dceb25c 100644 --- a/src/main/java/swervelib/imu/ADIS16470Swerve.java +++ b/src/main/java/swervelib/imu/ADIS16470Swerve.java @@ -2,8 +2,8 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; import edu.wpi.first.wpilibj.ADIS16470_IMU; +import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Optional; diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java index e2a8872e..74f5b9f7 100644 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -1,15 +1,14 @@ package swervelib.imu; import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.math.geometry.Quaternion; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.DriverStation; 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 java.util.Optional; +import swervelib.telemetry.Alert; /** * Communicates with the NavX as the IMU. @@ -29,6 +28,10 @@ public class NavXSwerve extends SwerveIMU * Inversion for the gyro */ private boolean invertedIMU = false; + /** + * An {@link Alert} for if there is an error instantiating the NavX. + */ + private Alert navXError; /** * Constructor for the NavX swerve. @@ -37,6 +40,7 @@ public class NavXSwerve extends SwerveIMU */ public NavXSwerve(SerialPort.Port port) { + navXError = new Alert("IMU", "Error instantiating NavX.", Alert.AlertType.ERROR_TRACE); try { /* Communicate w/navX-MXP via the MXP SPI Bus. */ @@ -47,7 +51,8 @@ public NavXSwerve(SerialPort.Port port) SmartDashboard.putData(gyro); } catch (RuntimeException ex) { - DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); + navXError.setText("Error instantiating NavX: " + ex.getMessage()); + navXError.set(true); } } @@ -68,7 +73,8 @@ public NavXSwerve(SPI.Port port) SmartDashboard.putData(gyro); } catch (RuntimeException ex) { - DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); + navXError.setText("Error instantiating NavX: " + ex.getMessage()); + navXError.set(true); } } @@ -89,7 +95,8 @@ public NavXSwerve(I2C.Port port) SmartDashboard.putData(gyro); } catch (RuntimeException ex) { - DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); + navXError.setText("Error instantiating NavX: " + ex.getMessage()); + navXError.set(true); } } diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java index 24361ce2..6b2e4ee9 100644 --- a/src/main/java/swervelib/imu/Pigeon2Swerve.java +++ b/src/main/java/swervelib/imu/Pigeon2Swerve.java @@ -138,7 +138,9 @@ public Optional getAccel() StatusSignal yAcc = imu.getAccelerationX(); StatusSignal zAcc = imu.getAccelerationX(); - return Optional.of(new Translation3d(xAcc.refresh().getValue(), yAcc.refresh().getValue(), zAcc.refresh().getValue()).times(9.81 / 16384.0)); + return Optional.of(new Translation3d(xAcc.refresh().getValue(), + yAcc.refresh().getValue(), + zAcc.refresh().getValue()).times(9.81 / 16384.0)); } /** diff --git a/src/main/java/swervelib/math/SwerveMath.java b/src/main/java/swervelib/math/SwerveMath.java index 0b4fdd5e..5de06d59 100644 --- a/src/main/java/swervelib/math/SwerveMath.java +++ b/src/main/java/swervelib/math/SwerveMath.java @@ -13,8 +13,6 @@ import swervelib.SwerveModule; import swervelib.parser.SwerveDriveConfiguration; import swervelib.parser.SwerveModuleConfiguration; -import swervelib.telemetry.SwerveDriveTelemetry; -import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; /** * Mathematical functions which pertain to swerve drive. diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index a5aef968..27c9c4a9 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -1,21 +1,21 @@ package swervelib.motors; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkMax; 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 edu.wpi.first.wpilibj.DriverStation; import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; +import swervelib.telemetry.Alert; import swervelib.telemetry.SwerveDriveTelemetry; /** @@ -27,11 +27,11 @@ public class SparkFlexSwerve extends SwerveMotor /** * SparkMAX Instance. */ - public CANSparkFlex motor; + public CANSparkFlex motor; /** * Integrated encoder. */ - public RelativeEncoder encoder; + public RelativeEncoder encoder; /** * Absolute encoder attached to the SparkMax (if exists) */ @@ -39,11 +39,19 @@ public class SparkFlexSwerve extends SwerveMotor /** * Closed-loop PID controller. */ - public SparkPIDController pid; + public SparkPIDController pid; /** * Factory default already occurred. */ private boolean factoryDefaultOccurred = false; + /** + * An {@link Alert} for if there is an error configuring the motor. + */ + 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; /** * Initialize the swerve motor. @@ -65,6 +73,15 @@ public SparkFlexSwerve(CANSparkFlex motor, boolean isDriveMotor) // 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); + 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); + } /** @@ -92,7 +109,7 @@ private void configureSparkFlex(Supplier config) return; } } - DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); + failureConfiguring.set(true); } /** @@ -185,10 +202,7 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) { - DriverStation.reportWarning( - "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the" + - " absoluteEncoderOffset in the Swerve Module JSON!", - false); + absoluteEncoderOffsetWarning.set(true); absoluteEncoder = encoder; configureSparkFlex(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder())); } @@ -357,20 +371,20 @@ public void setReference(double setpoint, double feedforward) if (isDriveMotor) { configureSparkFlex(() -> - pid.setReference( - setpoint, - ControlType.kVelocity, - pidSlot, - feedforward)); + pid.setReference( + setpoint, + ControlType.kVelocity, + pidSlot, + feedforward)); } else { configureSparkFlex(() -> - pid.setReference( - setpoint, - ControlType.kPosition, - pidSlot, - feedforward)); - if(SwerveDriveTelemetry.isSimulation) + pid.setReference( + setpoint, + ControlType.kPosition, + pidSlot, + feedforward)); + if (SwerveDriveTelemetry.isSimulation) { encoder.setPosition(setpoint); } diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index fc7d94ed..3ec65369 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -1,20 +1,20 @@ package swervelib.motors; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkMax; 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 edu.wpi.first.wpilibj.DriverStation; import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; +import swervelib.telemetry.Alert; /** * Brushed motor control with SparkMax. @@ -30,11 +30,11 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor /** * Absolute encoder attached to the SparkMax (if exists) */ - public AbsoluteEncoder absoluteEncoder; + public AbsoluteEncoder absoluteEncoder; /** * Integrated encoder. */ - public RelativeEncoder encoder; + public RelativeEncoder encoder; /** * Closed-loop PID controller. */ @@ -42,7 +42,19 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor /** * 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; + /** + * An {@link Alert} for if there is an error configuring the motor. + */ + private Alert failureConfiguringAlert; + /** + * An {@link Alert} for if the motor has no encoder defined. + */ + private Alert noEncoderDefinedAlert; /** * Initialize the swerve motor. @@ -59,7 +71,7 @@ public SparkMaxBrushedMotorSwerve(CANSparkMax motor, boolean isDriveMotor, Type // Drive motors **MUST** have an encoder attached. if (isDriveMotor && encoderType == Type.kNoSensor) { - DriverStation.reportError("Cannot use motor without encoder.", true); + noEncoderAlert.set(true); throw new RuntimeException("Cannot use SparkMAX as a drive motor without an encoder attached."); } @@ -90,6 +102,16 @@ public SparkMaxBrushedMotorSwerve(CANSparkMax motor, boolean isDriveMotor, Type } // Spin off configurations in a different thread. // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented it out because it prevents feedback. + + noEncoderAlert = new Alert("Motors", + "Cannot use motor without encoder.", + Alert.AlertType.ERROR_TRACE); + failureConfiguringAlert = new Alert("Motors", + "Failure configuring motor " + motor.getDeviceId(), + Alert.AlertType.WARNING_TRACE); + noEncoderDefinedAlert = new Alert("Motors", + "An encoder MUST be defined to work with a SparkMAX", + Alert.AlertType.ERROR_TRACE); } /** @@ -122,7 +144,7 @@ private void configureSparkMax(Supplier config) return; } } - DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); + failureConfiguringAlert.set(true); } /** @@ -220,7 +242,7 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) } if (absoluteEncoder == null && this.encoder == null) { - DriverStation.reportError("An encoder MUST be defined to work with a SparkMAX", true); + noEncoderDefinedAlert.set(true); throw new RuntimeException("An encoder MUST be defined to work with a SparkMAX"); } return this; diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index af29e21a..a5c0e22b 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -20,31 +20,31 @@ public class TalonFXSwerve extends SwerveMotor /** * Factory default already occurred. */ - private final boolean factoryDefaultOccurred = false; - /** - * Current TalonFX configuration. - */ - private TalonFXConfiguration configuration = new TalonFXConfiguration(); + 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); -// /** -// * Motion Magic exponential voltage setters. -// */ -// private final MotionMagicExpoVoltage m_angleVoltageExpoSetter = new MotionMagicExpoVoltage(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); + /** + * Conversion factor for the motor. + */ + private double conversionFactor; /** * TalonFX motor controller. */ TalonFX motor; + /** + * Current TalonFX configuration. + */ + private TalonFXConfiguration configuration = new TalonFXConfiguration(); /** * Constructor for TalonFX swerve motor. @@ -60,10 +60,10 @@ public TalonFXSwerve(TalonFX motor, boolean isDriveMotor) factoryDefaults(); clearStickyFaults(); -// if (SwerveDriveTelemetry.isSimulation) -// { -//// PhysicsSim.getInstance().addTalonFX(motor, .25, 6800); -// } + // if (SwerveDriveTelemetry.isSimulation) + // { + //// PhysicsSim.getInstance().addTalonFX(motor, .25, 6800); + // } } /** @@ -103,12 +103,12 @@ public void factoryDefaults() cfg.apply(configuration); m_angleVoltageSetter.UpdateFreqHz = 0; -// m_angleVoltageExpoSetter.UpdateFreqHz = 0; + // m_angleVoltageExpoSetter.UpdateFreqHz = 0; m_velocityVoltageSetter.UpdateFreqHz = 0; -// motor.configFactoryDefault(); -// motor.setSensorPhase(true); -// motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); -// motor.configNeutralDeadband(0.001); + // motor.configFactoryDefault(); + // motor.setSensorPhase(true); + // motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); + // motor.configNeutralDeadband(0.001); } } @@ -155,16 +155,23 @@ public void configureIntegratedEncoder(double positionConversionFactor) cfg.refresh(configuration); positionConversionFactor = 1 / positionConversionFactor; + if (!isDriveMotor) + { + positionConversionFactor *= 360; + } + conversionFactor = positionConversionFactor; - configuration.MotionMagic = configuration.MotionMagic - .withMotionMagicCruiseVelocity(100 / positionConversionFactor) - .withMotionMagicAcceleration((100 / positionConversionFactor) / 0.100) - .withMotionMagicExpo_kV(0.12 * positionConversionFactor) - .withMotionMagicExpo_kA(0.1); + configuration.MotionMagic = + configuration.MotionMagic.withMotionMagicCruiseVelocity(100.0 / positionConversionFactor) + .withMotionMagicAcceleration((100.0 / positionConversionFactor) / 0.100) + .withMotionMagicExpo_kV(0.12 * positionConversionFactor) + .withMotionMagicExpo_kA(0.1); - configuration.Feedback = configuration.Feedback - .withSensorToMechanismRatio(positionConversionFactor) - .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor); + configuration.Feedback = configuration.Feedback.withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor); + if (isDriveMotor) + { + configuration.Feedback = configuration.Feedback.withSensorToMechanismRatio(positionConversionFactor); + } cfg.apply(configuration); // Taken from democat's library. @@ -179,7 +186,7 @@ public void configureIntegratedEncoder(double positionConversionFactor) */ public void configureCANStatusFrames(int CANStatus1) { -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); } /** @@ -198,21 +205,31 @@ public void configureCANStatusFrames(int CANStatus1) * @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) + 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); + // 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 @@ -228,12 +245,10 @@ public void configurePIDF(PIDFConfig config) { TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.Slot0); - cfg.apply(configuration.Slot0.withKP(config.p) - .withKI(config.i) - .withKD(config.d) - .withKS(config.f)); -// configuration.slot0.integralZone = config.iz; -// configuration.slot0.closedLoopPeakOutput = config.output.max; + cfg.apply( + configuration.Slot0.withKP(config.p).withKI(config.i).withKD(config.d).withKS(config.f)); + // configuration.slot0.integralZone = config.iz; + // configuration.slot0.closedLoopPeakOutput = config.output.max; } /** @@ -270,7 +285,7 @@ public void setMotorBrake(boolean isBrakeMode) @Override public void setInverted(boolean inverted) { -// Timer.delay(1); + // Timer.delay(1); motor.setInverted(inverted); } @@ -316,17 +331,17 @@ public void setReference(double setpoint, double feedforward) @Override public void setReference(double setpoint, double feedforward, double position) { -// if (SwerveDriveTelemetry.isSimulation) -// { -// PhysicsSim.getInstance().run(); -// } + // if (SwerveDriveTelemetry.isSimulation) + // { + // PhysicsSim.getInstance().run(); + // } if (isDriveMotor) { motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint).withFeedForward(feedforward)); } else { - motor.setControl(m_angleVoltageSetter.withPosition(setpoint)); + motor.setControl(m_angleVoltageSetter.withPosition((setpoint - 180) / conversionFactor)); } } @@ -390,7 +405,9 @@ public void setCurrentLimit(int currentLimit) { TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.CurrentLimits); - cfg.apply(configuration.CurrentLimits.withStatorCurrentLimit(currentLimit).withStatorCurrentLimitEnable(true)); + cfg.apply( + configuration.CurrentLimits.withStatorCurrentLimit(currentLimit) + .withStatorCurrentLimitEnable(true)); } /** diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index 9b3a8aac..59e12053 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -26,6 +26,7 @@ import swervelib.motors.SwerveMotor; import swervelib.motors.TalonFXSwerve; import swervelib.motors.TalonSRXSwerve; +import swervelib.telemetry.Alert; /** * Device JSON parsed class. Used to access the JSON data. @@ -33,18 +34,30 @@ public class DeviceJson { + /** + * An {@link Alert} for if the CAN ID is greater than 40. + */ + private 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); + /** + * An {@link Alert} for if there is an I2C lockup issue on the roboRIO. + */ + private final Alert i2cLockupWarning = new Alert("IMU", + "I2C lockup issue detected on roboRIO. Check console for more information.", + Alert.AlertType.WARNING); /** * The device type, e.g. pigeon/pigeon2/sparkmax/talonfx/navx */ - public String type; + public String type; /** * The CAN ID or pin ID of the device. */ - public int id; + public int id; /** * The CAN bus name which the device resides on if using CAN. */ - public String canbus = ""; + public String canbus = ""; /** * Create a {@link SwerveAbsoluteEncoder} from the current configuration. @@ -57,8 +70,7 @@ public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor) { if (id > 40) { - DriverStation.reportWarning("CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", - false); + canIdWarning.set(true); } switch (type) { @@ -99,8 +111,7 @@ public SwerveIMU createIMU() { if (id > 40) { - DriverStation.reportWarning("CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", - false); + canIdWarning.set(true); } switch (type) { @@ -121,6 +132,7 @@ public SwerveIMU createIMU() "\nhttps://docs.wpilib.org/en/stable/docs/yearly-overview/known-issues" + ".html#onboard-i2c-causing-system-lockups", false); + i2cLockupWarning.set(true); return new NavXSwerve(I2C.Port.kMXP); case "navx_usb": return new NavXSwerve(Port.kUSB); @@ -145,8 +157,7 @@ public SwerveMotor createMotor(boolean isDriveMotor) { if (id > 40) { - DriverStation.reportWarning("CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", - false); + canIdWarning.set(true); } switch (type) { diff --git a/src/main/java/swervelib/telemetry/Alert.java b/src/main/java/swervelib/telemetry/Alert.java new file mode 100644 index 00000000..1718c82e --- /dev/null +++ b/src/main/java/swervelib/telemetry/Alert.java @@ -0,0 +1,213 @@ +// Copyright (c) 2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found below. + +// MIT License +// +// Copyright (c) 2023 FRC 6328 +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +package swervelib.telemetry; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.ArrayList; +import java.util.Comparator; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.function.Predicate; + +/** + * Class for managing persistent alerts to be sent over NetworkTables. + */ +public class Alert +{ + + private static Map groups = new HashMap(); + + private final AlertType type; + private boolean active = false; + private double activeStartTime = 0.0; + private String text; + + /** + * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, the appropriate + * entries will be added to NetworkTables. + * + * @param text Text to be displayed when the alert is active. + * @param type Alert level specifying urgency. + */ + public Alert(String text, AlertType type) + { + this("Alerts", text, type); + } + + /** + * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate entries will be added to + * NetworkTables. + * + * @param group Group identifier, also used as NetworkTables title + * @param text Text to be displayed when the alert is active. + * @param type Alert level specifying urgency. + */ + public Alert(String group, String text, AlertType type) + { + if (!groups.containsKey(group)) + { + groups.put(group, new SendableAlerts()); + SmartDashboard.putData(group, groups.get(group)); + } + + this.text = text; + this.type = type; + groups.get(group).alerts.add(this); + } + + /** + * Sets whether the alert should currently be displayed. When activated, the alert text will also be sent to the + * console. + */ + public void set(boolean active) + { + if (active && !this.active) + { + activeStartTime = Timer.getFPGATimestamp(); + switch (type) + { + case ERROR: + DriverStation.reportError(text, false); + break; + case ERROR_TRACE: + DriverStation.reportError(text, true); + break; + case WARNING: + DriverStation.reportWarning(text, false); + break; + case WARNING_TRACE: + DriverStation.reportWarning(text, true); + break; + case INFO: + System.out.println(text); + break; + } + } + this.active = active; + } + + /** + * Updates current alert text. + */ + public void setText(String text) + { + if (active && !text.equals(this.text)) + { + switch (type) + { + case ERROR: + DriverStation.reportError(text, false); + break; + case ERROR_TRACE: + DriverStation.reportError(text, true); + break; + case WARNING: + DriverStation.reportWarning(text, false); + break; + case WARNING_TRACE: + DriverStation.reportWarning(text, true); + break; + case INFO: + System.out.println(text); + break; + } + } + this.text = text; + } + + /** + * Represents an alert's level of urgency. + */ + public static enum AlertType + { + /** + * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type for problems which + * will seriously affect the robot's functionality and thus require immediate attention. + */ + ERROR, + /** + * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type for problems which + * will seriously affect the robot's functionality and thus require immediate attention. Trace printed to driver + * station console. + */ + ERROR_TRACE, + + /** + * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this type for problems + * which could affect the robot's functionality but do not necessarily require immediate attention. + */ + WARNING, + /** + * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this type for problems + * which could affect the robot's functionality but do not necessarily require immediate attention. Trace printed to + * driver station console. + */ + WARNING_TRACE, + /** + * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type for problems which + * are unlikely to affect the robot's functionality, or any other alerts which do not fall under "ERROR" or + * "WARNING". + */ + INFO + } + + private static class SendableAlerts implements Sendable + { + + public final List alerts = new ArrayList<>(); + + public String[] getStrings(AlertType type) + { + Predicate activeFilter = (Alert x) -> x.type == type && x.active; + Comparator timeSorter = + (Alert a1, Alert a2) -> (int) (a2.activeStartTime - a1.activeStartTime); + return alerts.stream() + .filter(activeFilter) + .sorted(timeSorter) + .map((Alert a) -> a.text) + .toArray(String[]::new); + } + + @Override + public void initSendable(SendableBuilder builder) + { + builder.setSmartDashboardType("Alerts"); + builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null); + builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR_TRACE), null); + builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null); + builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING_TRACE), null); + builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null); + } + } +} \ No newline at end of file