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