diff --git a/simgui.json b/simgui.json index 005c0cb..f5066c8 100644 --- a/simgui.json +++ b/simgui.json @@ -34,6 +34,9 @@ } }, "NetworkTables": { + "Retained Values": { + "open": false + }, "transitory": { "Shuffleboard": { "Arm": { diff --git a/src/main/deploy/pathplanner/autos/+5 spin 90 -5.auto b/src/main/deploy/pathplanner/autos/+5 spin 90 -5.auto new file mode 100644 index 0000000..cf0d2a3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/+5 spin 90 -5.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2.0, + "y": 7.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "+5 spin 90" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "-5m" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/+5m -5m.auto b/src/main/deploy/pathplanner/autos/+5m -5m.auto new file mode 100644 index 0000000..a834275 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/+5m -5m.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 7.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "+5m" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "-5m" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Calibration.auto b/src/main/deploy/pathplanner/autos/+5m.auto similarity index 85% rename from src/main/deploy/pathplanner/autos/Calibration.auto rename to src/main/deploy/pathplanner/autos/+5m.auto index 993c91b..1e1b6ca 100644 --- a/src/main/deploy/pathplanner/autos/Calibration.auto +++ b/src/main/deploy/pathplanner/autos/+5m.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 2.0, + "x": 2, "y": 7.0 }, "rotation": 0 @@ -14,7 +14,7 @@ { "type": "path", "data": { - "pathName": "3m Backwards" + "pathName": "+5m" } } ] diff --git a/src/main/deploy/pathplanner/paths/+5 spin 90.path b/src/main/deploy/pathplanner/paths/+5 spin 90.path new file mode 100644 index 0000000..9995b33 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/+5 spin 90.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0, + "y": 7.0 + }, + "prevControl": { + "x": 6.0, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3m Backwards.path b/src/main/deploy/pathplanner/paths/+5m.path similarity index 90% rename from src/main/deploy/pathplanner/paths/3m Backwards.path rename to src/main/deploy/pathplanner/paths/+5m.path index f03256c..a0916da 100644 --- a/src/main/deploy/pathplanner/paths/3m Backwards.path +++ b/src/main/deploy/pathplanner/paths/+5m.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 5.0, + "x": 7.0, "y": 7.0 }, "prevControl": { - "x": 3.479309367425449, + "x": 6.88, "y": 7.0 }, "nextControl": null, @@ -45,5 +45,5 @@ "reversed": false, "folder": null, "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/-5m.path b/src/main/deploy/pathplanner/paths/-5m.path new file mode 100644 index 0000000..b40caea --- /dev/null +++ b/src/main/deploy/pathplanner/paths/-5m.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 8.118033988749895, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": { + "x": 1.88, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/arm/ArmFactory.java b/src/main/java/frc/robot/arm/ArmFactory.java index 17940c7..5880546 100644 --- a/src/main/java/frc/robot/arm/ArmFactory.java +++ b/src/main/java/frc/robot/arm/ArmFactory.java @@ -11,7 +11,7 @@ public class ArmFactory { * @return a shoulder motor. */ public static ShoulderMotorIO createShoulderMotor() { - if (Robot.isReal()) return new ShoulderMotorIOSparkMax(); + // if (Robot.isReal()) return new ShoulderMotorIOSparkMax(); return new ShoulderMotorIOSim(); } diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index cd85112..c66a9ea 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -26,20 +26,30 @@ public class Auto extends Subsystem { /** Instance variable for the auto subsystem singleton. */ private static Auto instance = null; + /** Reference to the odometry subsystem. */ + private final Odometry odometry; + + /** Reference to the swerve subsystem. */ + private final Swerve swerve; + + /** Sendable chooser for the subsystem. */ private final SendableChooser autoChooser; /** Creates a new instance of the auto subsystem. */ private Auto() { - Supplier robotPositionSupplier = () -> Odometry.getInstance().getPosition(); + odometry = Odometry.getInstance(); + swerve = Swerve.getInstance(); + + Supplier robotPositionSupplier = () -> odometry.getPosition(); Consumer robotPositionConsumer = - position -> Odometry.getInstance().setPosition(position); + position -> odometry.setPosition(position); Supplier swerveChassisSpeedsSupplier = - () -> Swerve.getInstance().getChassisSpeeds(); + () -> swerve.getChassisSpeeds(); Consumer swerveChassisSpeedsConsumer = - chassisSpeeds -> Swerve.getInstance().setChassisSpeeds(chassisSpeeds); + chassisSpeeds -> swerve.setChassisSpeeds(chassisSpeeds); HolonomicPathFollowerConfig holonomicPathFollowerConfig = new HolonomicPathFollowerConfig( @@ -56,7 +66,7 @@ private Auto() { swerveChassisSpeedsConsumer, holonomicPathFollowerConfig, this::shouldFlipPath, - this); + swerve); autoChooser = AutoBuilder.buildAutoChooser(); } diff --git a/src/main/java/frc/robot/lights/LightsFactory.java b/src/main/java/frc/robot/lights/LightsFactory.java index 392494f..0eabdff 100644 --- a/src/main/java/frc/robot/lights/LightsFactory.java +++ b/src/main/java/frc/robot/lights/LightsFactory.java @@ -1,5 +1,7 @@ package frc.robot.lights; +import frc.robot.Robot; + /** Helper class for creating hardware for the lights subsystem. */ public class LightsFactory { @@ -9,7 +11,7 @@ public class LightsFactory { * @return an LED controller. */ public static LEDControllerIO createLEDController() { - // if (Robot.isReal()) return new LEDControllerIOCANdle(); + if (Robot.isReal()) return new LEDControllerIOCANdle(); return new LEDControllerIOSim(); } diff --git a/src/main/java/frc/robot/odometry/OdometryFactory.java b/src/main/java/frc/robot/odometry/OdometryFactory.java index 987f7a3..1290178 100644 --- a/src/main/java/frc/robot/odometry/OdometryFactory.java +++ b/src/main/java/frc/robot/odometry/OdometryFactory.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.configs.Pigeon2Configuration; import frc.lib.CAN; +import frc.robot.Robot; /** Helper class for creating hardware for the odometry subsystem. */ public class OdometryFactory { @@ -12,7 +13,7 @@ public class OdometryFactory { * @return a gyroscope. */ public static GyroscopeIO createGyroscope(CAN gyroscopeCAN, Odometry odometry) { - // if (Robot.isReal()) return new GyroscopeIOPigeon2(gyroscopeCAN); + if (Robot.isReal()) return new GyroscopeIOPigeon2(gyroscopeCAN); return new GyroscopeIOSim(odometry); } diff --git a/src/main/java/frc/robot/swerve/DriveCommand.java b/src/main/java/frc/robot/swerve/DriveCommand.java index b42924c..111ec8d 100644 --- a/src/main/java/frc/robot/swerve/DriveCommand.java +++ b/src/main/java/frc/robot/swerve/DriveCommand.java @@ -53,7 +53,13 @@ public DriveCommand(CommandXboxController driverController) { } @Override - public void initialize() {} + public void initialize() { + Rotation2d poseHeading = odometry.getPosition().getRotation(); + + // TODO Determine which of these, if not both, need to be set to prevent immediate spinning + headingGoal = wrap(new State(poseHeading.getRotations(), 0.0), poseHeading.getRotations()); + headingSetpoint = wrap(new State(poseHeading.getRotations(), 0.0), poseHeading.getRotations()); + } @Override public void execute() { diff --git a/src/main/java/frc/robot/swerve/SwerveConstants.java b/src/main/java/frc/robot/swerve/SwerveConstants.java index 9fc7a88..5d9d0de 100644 --- a/src/main/java/frc/robot/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/swerve/SwerveConstants.java @@ -25,14 +25,14 @@ public static class MK4iConstants { public static final double WHEEL_DIAMETER = Units.inchesToMeters(4.0); /** Actual distance driven during the odometry test in meters. */ - public static final double ODOMETRY_TEST_ACTUAL_DISTANCE = 6.985; + public static final double ODOMETRY_TEST_ACTUAL_DISTANCE = 5.49; /** Reported distance driven during the odometry test in meters. */ - public static final double ODOMETRY_TEST_REPORTED_DISTANCE = 8.219; + public static final double ODOMETRY_TEST_REPORTED_DISTANCE = 5.0; /** Conversion between odometry distance and actual distance travelled. */ public static final double ODOMETRY_ERROR = - ODOMETRY_TEST_ACTUAL_DISTANCE / ODOMETRY_TEST_REPORTED_DISTANCE; + ODOMETRY_TEST_REPORTED_DISTANCE / ODOMETRY_TEST_ACTUAL_DISTANCE; /** Conversion between wheel rotations and distances in meters. */ public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI * ODOMETRY_ERROR; diff --git a/src/main/java/frc/robot/swerve/SwerveFactory.java b/src/main/java/frc/robot/swerve/SwerveFactory.java index 4585e94..8021550 100644 --- a/src/main/java/frc/robot/swerve/SwerveFactory.java +++ b/src/main/java/frc/robot/swerve/SwerveFactory.java @@ -23,8 +23,8 @@ public static SwerveModuleIO createModule(SwerveModuleConfig config) { * @return an azimuth encoder. */ public static AzimuthEncoderIO createAzimuthEncoder(SwerveModuleConfig config) { - // if (Robot.isReal()) - // return new AzimuthEncoderIOCANcoder(config.moduleCAN().azimuth(), config.offset()); + if (Robot.isReal()) + return new AzimuthEncoderIOCANcoder(config.moduleCAN().azimuth(), config.offset()); return new AzimuthEncoderIOSim(); } @@ -46,9 +46,9 @@ public static CANcoderConfiguration createAzimuthEncoderConfig() { * @return a steer motor. */ public static SteerMotorIO createSteerMotor(SwerveModuleConfig config) { - // if (Robot.isReal()) - // return new SteerMotorIOTalonFXPIDF(config.moduleCAN().steer(), - // config.moduleCAN().azimuth()); + if (Robot.isReal()) + return new SteerMotorIOTalonFXPIDF(config.moduleCAN().steer(), + config.moduleCAN().azimuth()); return new SteerMotorIOSim(); } @@ -59,7 +59,7 @@ public static SteerMotorIO createSteerMotor(SwerveModuleConfig config) { * @return a drive motor. */ public static DriveMotorIO createDriveMotor(SwerveModuleConfig config) { - // if (Robot.isReal()) return new DriveMotorIOTalonFXPID(config.moduleCAN().drive()); + if (Robot.isReal()) return new DriveMotorIOTalonFXPID(config.moduleCAN().drive()); return new DriveMotorIOSim(); } diff --git a/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java b/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java index a20f77e..0a3bbaf 100644 --- a/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java +++ b/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java @@ -1,5 +1,6 @@ package frc.robot.swerve; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -25,9 +26,12 @@ public class SwerveModuleIOCustom implements SwerveModuleIO { /** Drive motor. */ private final DriveMotorIO driveMotor; - /** Driver motor values. */ + /** Drive motor values. */ private final DriveMotorIOValues driveMotorValues = new DriveMotorIOValues(); + /** Speeds below this speed are zeroed. */ + private final double kSpeedDeadbandMetersPerSecond = 0.025; + /** Module setpoint */ private SwerveModuleState setpoint; @@ -76,9 +80,7 @@ public SwerveModuleState getSetpoint() { @Override public void setSetpoint(SwerveModuleState setpoint, boolean lazy) { - if (lazy) { - setpoint = optimize(setpoint, getState()); - } + setpoint = optimize(setpoint, getState(), lazy); steerMotor.setSetpoint(setpoint.angle.getRotations()); driveMotor.setSetpoint(setpoint.speedMetersPerSecond); @@ -91,19 +93,34 @@ public void setSetpoint(SwerveModuleState setpoint, boolean lazy) { * * @param setpoint the setpoint to optimize. * @param state the state of the module. + * @param lazy if true, perform additional optimizations on the setpoint. * @return the optimized setpoint. */ - private SwerveModuleState optimize(SwerveModuleState setpoint, SwerveModuleState state) { + private SwerveModuleState optimize(SwerveModuleState setpoint, SwerveModuleState state, boolean lazy) { + // Always perform this optimization, even when lazy setpoint = SwerveModuleState.optimize(setpoint, state.angle); + // If we aren't lazy, don't perform additional optimizations + if (!lazy) { + return setpoint; + } + + // Since we are lazy, perform additional optimizations + + // Deadband the module speed + if (MathUtil.isNear(0.0, setpoint.speedMetersPerSecond, kSpeedDeadbandMetersPerSecond)) { + setpoint.speedMetersPerSecond = 0.0; + } + + // Keep previous angle if we aren't moving if (setpoint.speedMetersPerSecond == 0.0) { setpoint.angle = state.angle; - } else { - Rotation2d angleError = setpoint.angle.minus(state.angle); - - setpoint.speedMetersPerSecond *= angleError.getCos(); } + // Scale our speed by the module's error + Rotation2d error = setpoint.angle.minus(state.angle); + setpoint.speedMetersPerSecond *= error.getCos(); + return setpoint; }