diff --git a/src/main/java/com/overture/lib/gamepads/OverXboxController.java b/src/main/java/com/overture/lib/gamepads/OverXboxController.java index 8df9080..67b6e3b 100644 --- a/src/main/java/com/overture/lib/gamepads/OverXboxController.java +++ b/src/main/java/com/overture/lib/gamepads/OverXboxController.java @@ -143,69 +143,6 @@ public Command getRumbleCommand(double intensity) { return Commands.runOnce(() -> setRumble(GenericHID.RumbleType.kBothRumble, intensity)); } - /** - * Returns a trigger for the left stick being pushed past a threshold on the y axis. - * - * @param triggerThreshold the threshold to exceed - * @return the trigger - */ - public Trigger leftYTrigger(double triggerThreshold) { - return new Trigger(() -> Math.abs(getLeftY()) >= triggerThreshold); - } - - /** - * Returns a trigger for the left stick being pushed past a threshold on the x axis. - * - * @param triggerThreshold the threshold to exceed - * @return the trigger - */ - public Trigger leftXTrigger(double triggerThreshold) { - return new Trigger(() -> Math.abs(getLeftX()) >= triggerThreshold); - } - - /** - * Returns a trigger for the right stick being pushed past a threshold on the y axis. - * - * @param triggerThreshold the threshold to exceed - * @return the trigger - */ - public Trigger rightYTrigger(double triggerThreshold) { - return new Trigger(() -> Math.abs(getRightY()) >= triggerThreshold); - } - - /** - * Returns a trigger for the right stick being pushed past a threshold on the x axis. - * - * @param triggerThreshold the threshold to exceed - * @return the trigger - */ - public Trigger rightXTrigger(double triggerThreshold) { - return new Trigger(() -> Math.abs(getRightX()) >= triggerThreshold); - } - - /** - * Returns a trigger for the right stick being pushed past a threshold on either axis. - * - * @param triggerThreshold the threshold to exceed - * @return the trigger - */ - public Trigger rightStick(double triggerThreshold) { - return new Trigger( - () -> - Math.abs(getRightX()) >= triggerThreshold || Math.abs(getRightY()) >= triggerThreshold); - } - - /** - * Returns a trigger for the left stick being pushed past a threshold on either axis. - * - * @param triggerThreshold the threshold to exceed - * @return the trigger - */ - public Trigger leftStick(double triggerThreshold) { - return new Trigger( - () -> Math.abs(getLeftX()) >= triggerThreshold || Math.abs(getLeftY()) >= triggerThreshold); - } - /** Publishes the connection state of the controller. */ public void updateTelemetry() { isConnectedAlert.set(!isConnected()); diff --git a/src/main/java/com/overture/lib/robots/OverContainer.java b/src/main/java/com/overture/lib/robots/OverContainer.java index 1c50f59..6b6fead 100644 --- a/src/main/java/com/overture/lib/robots/OverContainer.java +++ b/src/main/java/com/overture/lib/robots/OverContainer.java @@ -12,9 +12,6 @@ public interface OverContainer { /** Configures the operator's button bindings. */ void configOperatorBindings(); - /** Configures the default commands of every subsystem. */ - void configDefaultCommands(); - /** Configures the bindings used for characterization routines. */ void configCharacterizationBindings(); diff --git a/src/main/java/com/overture/lib/subsystems/swerve/SwerveChassis.java b/src/main/java/com/overture/lib/subsystems/swerve/SwerveChassis.java index 2b135ca..581d30b 100644 --- a/src/main/java/com/overture/lib/subsystems/swerve/SwerveChassis.java +++ b/src/main/java/com/overture/lib/subsystems/swerve/SwerveChassis.java @@ -12,6 +12,7 @@ import com.overture.lib.motorcontrollers.ControllerNeutralMode; import com.overture.lib.robots.RobotConstants; import com.overture.lib.utils.Logging; +import com.overture.lib.utils.UtilityFunctions; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -39,14 +40,18 @@ public abstract class SwerveChassis extends SwerveBase { private ChassisSpeeds lastSpeeds = new ChassisSpeeds(); private ChassisAccels currentAccels = new ChassisAccels(); - // Thresholds are a starting point, not a tuned value: 1.3 means the fastest module may read 30% - // above the slowest before anyone is accused, and below 0.3 m/s of translation there is not + // Thresholds are a starting point, not a tuned value: 1.3 means the fastest + // module may read 30% + // above the slowest before anyone is accused, and below 0.3 m/s of translation + // there is not // enough motion for the ratio to mean anything. private final SkidDetector skidDetector = new SkidDetector(1.3, 0.3); private SkidDetector.Result skidResult = SkidDetector.Result.none(); - // 1.5 g is above what a swerve pulls on its own and below the Pigeon 2 rail at 2 g. Held for a - // quarter second, because a hit is over in a loop or two and nothing downstream samples that + // 1.5 g is above what a swerve pulls on its own and below the Pigeon 2 rail at + // 2 g. Held for a + // quarter second, because a hit is over in a loop or two and nothing downstream + // samples that // fast. private final CollisionDetector collisionDetector = new CollisionDetector(1.5, 0.25); @@ -150,9 +155,9 @@ protected double getAngularVelocityRadiansPerSecond() { *
    * @Override
    * protected double getMeasuredAccelerationGs() {
-   *   return Math.hypot(
-   *       pigeon.getAccelerationX().getValueAsDouble(),
-   *       pigeon.getAccelerationY().getValueAsDouble());
+   * 	return Math.hypot(
+   * 			pigeon.getAccelerationX().getValueAsDouble(),
+   * 			pigeon.getAccelerationY().getValueAsDouble());
    * }
    * 
* @@ -182,11 +187,16 @@ public Pose2d getEstimatedPose() { @Override public void resetOdometry(Pose2d initPose) { odometry.resetPosition(getRotation2d(), modulesPositions, initPose); - // Refresh the cache getEstimatedPose() serves. Without this the estimator knows the new pose - // but every reader keeps seeing the old one until the next periodic(), and CommandScheduler - // runs subsystem periodics BEFORE it initializes commands. PathPlanner registers - // resetOdometry as its reset consumer and getEstimatedPose as its pose supplier, so a - // reset-then-follow scheduled from a trigger would start the path from the pre-reset pose. + // Refresh the cache getEstimatedPose() serves. Without this the estimator knows + // the new pose + // but every reader keeps seeing the old one until the next periodic(), and + // CommandScheduler + // runs subsystem periodics BEFORE it initializes commands. PathPlanner + // registers + // resetOdometry as its reset consumer and getEstimatedPose as its pose + // supplier, so a + // reset-then-follow scheduled from a trigger would start the path from the + // pre-reset pose. latestPose = odometry.getEstimatedPosition(); } @@ -241,6 +251,11 @@ public void resetHeading(double angleDegrees) { resetOdometry(newOdometry); } + public Command resetHeadingCommand() { + double headingDegrees = UtilityFunctions.isRedAlliance() ? 180.0 : 0.0; + return Commands.runOnce(() -> this.resetHeading(headingDegrees)); + } + /** * Points the wheels into an X so the robot resists being pushed. * @@ -267,7 +282,8 @@ public void setXMode(boolean enabled) { @Override public void setTargetSpeeds(ChassisSpeeds speeds) { - // Copied rather than aliased. A caller usually owns one ChassisSpeeds it refills every loop, + // Copied rather than aliased. A caller usually owns one ChassisSpeeds it + // refills every loop, // and periodic() hands a speeds helper something it may rewrite in place. desiredSpeeds = new ChassisSpeeds( @@ -289,8 +305,10 @@ private double getPeriodicPeriod() { double period = now - lastPeriodicTimestamp; lastPeriodicTimestamp = now; - // First call, and the gap either side of a SysId routine, land outside anything plausible. Fall - // back to the configured period instead of discretizing by zero, which is a silent no-op. + // First call, and the gap either side of a SysId routine, land outside anything + // plausible. Fall + // back to the configured period instead of discretizing by zero, which is a + // silent no-op. return period > 0.0 && period < 0.5 ? period : RobotConstants.kLoopTime; } @@ -354,15 +372,21 @@ private void updateOdometry(double periodSeconds) { latestPose = odometry.getEstimatedPosition(); currentSpeeds = getKinematics().toChassisSpeeds(modulesStates); - // Differentiated over the period actually measured. The two argument constructor assumes 20 ms, - // so on a robot scheduling its loop faster every reported acceleration came out scaled by the - // ratio -- half, at a 10 ms loop -- and TargetingWhileMoving under-corrected by the same + // Differentiated over the period actually measured. The two argument + // constructor assumes 20 ms, + // so on a robot scheduling its loop faster every reported acceleration came out + // scaled by the + // ratio -- half, at a 10 ms loop -- and TargetingWhileMoving under-corrected by + // the same // factor. currentAccels = new ChassisAccels(currentSpeeds, lastSpeeds, periodSeconds); - // Measured only, for now: nothing downstream acts on it. Watch /Swerve/Chassis/SkidRatio on a - // real log first, because the threshold that separates a slipping wheel from a carpet seam is - // a property of your carpet and your wheels, not something to guess in a library. + // Measured only, for now: nothing downstream acts on it. Watch + // /Swerve/Chassis/SkidRatio on a + // real log first, because the threshold that separates a slipping wheel from a + // carpet seam is + // a property of your carpet and your wheels, not something to guess in a + // library. skidResult = skidDetector.detect( modulesStates, getKinematics().getModules(), getAngularVelocityRadiansPerSecond()); @@ -424,16 +448,6 @@ public void periodic() { throw new IllegalStateException("Have not called SwerveBase.configureSwerveBase!!!"); } - // Read the modules here, first, rather than leaving it to their own periodic. They register - // with the CommandScheduler after this class does -- a subclass's fields are initialised after - // its super constructor runs, and the scheduler keeps subsystems in insertion order -- so their - // periodic runs after this one. Everything below was therefore working from module data one - // full loop old, which is invisible in the odometry error because the wheels are what odometry - // measures: the pose was self consistent and simply late, and vision fused against it with an - // honest timestamp and a stale baseline. - // - // Above the characterizing check on purpose. SysId logs module position and velocity, so the - // modules have to be current even on the loops where the rest of this is skipped. getFrontLeftModule().updateInputs(); getFrontRightModule().updateInputs(); getBackLeftModule().updateInputs(); @@ -443,13 +457,8 @@ public void periodic() { return; } - // Measured once and shared. Reading it twice would hand the second caller the time since the - // first read, which is almost nothing. double periodSeconds = getPeriodicPeriod(); - // The helper gets a copy, so desiredSpeeds stays the request that came in. It is the signal - // that gets logged, and on a loop where nothing called setTargetSpeeds a helper would - // otherwise be reading back its own previous answer. ChassisSpeeds targetSpeeds = new ChassisSpeeds( desiredSpeeds.vxMetersPerSecond, @@ -467,11 +476,6 @@ public void periodic() { modulesStates[2] = getBackLeftModule().getState(); modulesStates[3] = getBackRightModule().getState(); - // Discretized here, last, rather than when the speeds arrived. discretize counter-rotates vx - // and vy to pay for the heading change across one period, so it has to see the omega that is - // actually about to be commanded. A heading helper replaces omega outright, so compensating - // for the driver's requested omega sent the robot sideways whenever it auto-aimed while - // translating: the correction was paying for a rotation that was not happening. SwerveModuleState[] desiredStates = getKinematics().toSwerveModuleStates(ChassisSpeeds.discretize(targetSpeeds, periodSeconds)); SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, getMaxModuleSpeed());