More changes from WPI day 2

Mechanical-Advantage · Feb 12, 2024 · 903ef58 · 903ef58
1 parent 802c31c
commit 903ef58
Show file tree

Hide file tree

Showing 10 changed files with 59 additions and 42 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
@@ -252,7 +252,7 @@ private void configureButtonBindings() {
 
     if (superstructure != null) {
       controller
-          .leftTrigger()
+          .a()
           .onTrue(
               Commands.runOnce(
                   () -> superstructure.setGoalState(Superstructure.SystemState.PREPARE_SHOOT)))
@@ -276,8 +276,10 @@ private void configureButtonBindings() {
                       () -> {
                         superstructure.setGoalState(Superstructure.SystemState.SHOOT);
                         rollers.setGoal(Rollers.Goal.FEED_SHOOTER);
-                      })
-                  .andThen(Commands.waitSeconds(0.5))
+                      },
+                      superstructure,
+                      rollers)
+                  .andThen(Commands.waitSeconds(1.0))
                   .andThen(
                       Commands.runOnce(
                           () -> {
@@ -293,7 +295,8 @@ private void configureButtonBindings() {
                       superstructure)
                   .andThen(
                       Commands.waitSeconds(0.25),
-                      Commands.runOnce(() -> rollers.setGoal(Rollers.Goal.FLOOR_INTAKE), rollers))
+                      Commands.runOnce(() -> rollers.setGoal(Rollers.Goal.FLOOR_INTAKE), rollers),
+                      Commands.idle())
                   .finallyDo(
                       () -> {
                         rollers.setGoal(Rollers.Goal.IDLE);
@@ -308,7 +311,8 @@ private void configureButtonBindings() {
                       superstructure)
                   .andThen(
                       Commands.waitSeconds(0.25),
-                      Commands.runOnce(() -> rollers.setGoal(Rollers.Goal.EJECT_TO_FLOOR), rollers))
+                      Commands.runOnce(() -> rollers.setGoal(Rollers.Goal.EJECT_TO_FLOOR), rollers),
+                      Commands.idle())
                   .finallyDo(
                       () -> {
                         rollers.setGoal(Rollers.Goal.IDLE);

diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java
@@ -35,7 +35,7 @@ public record AimingParameters(
       new LoggedTunableNumber("RobotState/lookaheadS", 0.0);
 
   private static LoggedTunableNumber shotHeightCompensation =
-      new LoggedTunableNumber("Superstructure/CompensationInches", 6.0);
+      new LoggedTunableNumber("RobotState/CompensationMeters", 0.55);
 
   private static final double poseBufferSizeSeconds = 2.0;
 

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java
@@ -79,6 +79,9 @@ public static class OdometryTimestampInputs {
   private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
   private final Module[] modules = new Module[4];
 
+  private SwerveDriveWheelPositions lastPositions = null;
+  private double lastTime = 0.0;
+
   /** Active drive mode. */
   private DriveMode currentDriveMode = DriveMode.TELEOP;
 
@@ -154,6 +157,7 @@ public void periodic() {
     }
     // Pass odometry data to robot state
     for (int i = 0; i < minOdometryUpdates; i++) {
+      boolean includeMeasurement = true;
       int odometryIndex = i;
       Rotation2d yaw = gyroInputs.connected ? gyroInputs.odometryYawPositions[i] : null;
       // Get all four swerve module positions at that odometry update
@@ -163,12 +167,34 @@ public void periodic() {
               Arrays.stream(modules)
                   .map(module -> module.getModulePositions()[odometryIndex])
                   .toArray(SwerveModulePosition[]::new));
-      // Add observation to robot state
-      RobotState.getInstance()
-          .addOdometryObservation(
-              new RobotState.OdometryObservation(
-                  wheelPositions, yaw, odometryTimestampInputs.timestamps[i]));
+      // Filtering
+      if (lastPositions != null) {
+        double dt = Timer.getFPGATimestamp() - lastTime;
+        for (int j = 0; j < 4; j++) {
+          double velocity =
+              (wheelPositions.positions[j].distanceMeters
+                      - lastPositions.positions[j].distanceMeters)
+                  / dt;
+          double omega =
+              wheelPositions.positions[j].angle.minus(lastPositions.positions[j].angle).getRadians()
+                  / dt;
+
+          if (Math.abs(omega) > currentModuleLimits.maxSteeringVelocity() * 10.0
+              || Math.abs(velocity) > currentModuleLimits.maxDriveVelocity() * 10.0) {
+            includeMeasurement = false;
+            break;
+          }
+        }
+      }
+      if (includeMeasurement) {
+        lastPositions = wheelPositions;
+        RobotState.getInstance()
+            .addOdometryObservation(
+                new RobotState.OdometryObservation(
+                    wheelPositions, yaw, odometryTimestampInputs.timestamps[i]));
+      }
     }
+    lastTime = Timer.getFPGATimestamp();
 
     // update current velocities use gyro when possible
     ChassisSpeeds robotRelativeVelocity = getSpeeds();

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java
@@ -50,7 +50,11 @@ public void periodic() {
       case STATION_INTAKE -> {
         indexer.setGoal(Indexer.Goal.STATION_INTAKING);
       }
-      case FEED_SHOOTER -> indexer.setGoal(Indexer.Goal.SHOOTING);
+      case FEED_SHOOTER -> {
+        indexer.setGoal(Indexer.Goal.SHOOTING);
+        intake.setGoal(Intake.Goal.SHOOTING);
+        feeder.setGoal(Feeder.Goal.SHOOTING);
+      }
       case EJECT_TO_FLOOR -> {
         feeder.setGoal(Feeder.Goal.EJECTING);
         indexer.setGoal(Indexer.Goal.EJECTING);

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java
@@ -14,6 +14,7 @@ public enum Goal implements GenericRollerSubsystem.VoltageGoal {
     IDLE(() -> 0.0),
     FLOOR_INTAKING(new LoggedTunableNumber("Feeder/FloorIntakingVoltage", 8.0)),
     BACKSTOPPING(new LoggedTunableNumber("Feeder/BackstoppingVoltage", -4.0)),
+    SHOOTING(new LoggedTunableNumber("Feeder/Shooting", 8.0)),
     EJECTING(new LoggedTunableNumber("Feeder/EjectingVoltage", -6.0));
 
     private final DoubleSupplier voltageSupplier;

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java
@@ -13,6 +13,7 @@ public class Intake extends GenericRollerSubsystem<Intake.Goal> {
   public enum Goal implements VoltageGoal {
     IDLE(() -> 0.0),
     FLOOR_INTAKING(new LoggedTunableNumber("Intake/FloorIntakingVoltage", 8.0)),
+    SHOOTING(new LoggedTunableNumber("Intake/Shooting", 6.0)),
     EJECTING(new LoggedTunableNumber("Intake/EjectingVoltage", -8.0));
 
     private final DoubleSupplier voltageSupplier;

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java
@@ -21,8 +21,6 @@ public class Superstructure extends SubsystemBase {
       new LoggedTunableNumber("Superstructure/ArmStationIntakeSetpointDegrees", 45.0);
   private static LoggedTunableNumber armIntakeSetpointDegrees =
       new LoggedTunableNumber("Superstructure/ArmIntakeDegrees", 40.0);
-  private static LoggedTunableNumber yCompensation =
-      new LoggedTunableNumber("Superstructure/CompensationMeters", 0.55);
   private static LoggedTunableNumber followThroughTime =
       new LoggedTunableNumber("Superstructure/FollowthroughTimeSecs", 0.5);
 
@@ -57,31 +55,11 @@ public enum GamepieceState {
   @Override
   public void periodic() {
     switch (goalState) {
-      case IDLE -> {
-        if (currentState == SystemState.SHOOT) {
-          if (followThroughTimer.hasElapsed(followThroughTime.get())) {
-            currentState = SystemState.IDLE;
-            followThroughTimer.stop();
-            followThroughTimer.reset();
-          } else {
-            currentState = SystemState.SHOOT;
-          }
-        } else {
-          currentState = SystemState.IDLE;
-        }
-      }
+      case IDLE -> currentState = SystemState.IDLE;
       case STATION_INTAKE -> currentState = SystemState.STATION_INTAKE;
       case INTAKE -> currentState = SystemState.INTAKE;
       case PREPARE_SHOOT -> currentState = SystemState.PREPARE_SHOOT;
-      case SHOOT -> {
-        if (currentState != SystemState.PREPARE_SHOOT) {
-          currentState = SystemState.PREPARE_SHOOT;
-        } else if (atShootingSetpoint()) {
-          currentState = SystemState.SHOOT;
-          followThroughTimer.restart();
-          goalState = SystemState.IDLE;
-        }
-      }
+      case SHOOT -> currentState = SystemState.SHOOT;
     }
 
     switch (currentState) {
@@ -106,7 +84,9 @@ public void periodic() {
         flywheels.setGoal(Flywheels.Goal.SHOOTING);
       }
       case SHOOT -> {
-        gamepieceState = GamepieceState.NO_GAMEPIECE;
+        var aimingParams = RobotState.getInstance().getAimingParameters();
+        flywheels.setGoal(Flywheels.Goal.SHOOTING);
+        arm.setSetpoint(aimingParams.armAngle());
       }
     }
 

diff --git a/...java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java b/...java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java
@@ -10,7 +10,7 @@ public class SuperstructureConstants {
   public static class FlywheelConstants {
     // encoder / flywheelReduction = flywheel
     public static double reduction = (1.0 / 2.0);
-    public static double shooterToleranceRPM = 50.0;
+    public static double shooterToleranceRPM = 100.0;
     public static int leftID = 5;
     public static int rightID = 4;
 

diff --git a/...ain/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java b/...ain/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java
@@ -82,7 +82,7 @@ public void periodic() {
       setGoal(Goal.IDLE);
     } else {
       switch (goal) {
-        case IDLE -> setSetpoint(idleLeftRPM.get(), idleRightRPM.get());
+        case IDLE -> io.stop();
         case INTAKING -> setSetpoint(intakingLeftRPM.get(), intakingRightRPM.get());
         case SHOOTING -> setSetpoint(shootingLeftRPM.get(), shootingRightRPM.get());
       }

diff --git a/...g/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSparkFlex.java b/...g/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSparkFlex.java
@@ -86,13 +86,13 @@ public void runVolts(double leftVolts, double rightVolts) {
   @Override
   public void runVelocity(double leftRpm, double rightRpm) {
     leftController.setReference(
-        leftRpm,
+        leftRpm * reduction,
         CANSparkBase.ControlType.kVelocity,
         0,
         ff.calculate(leftRpm),
         SparkPIDController.ArbFFUnits.kVoltage);
     rightController.setReference(
-        rightRpm,
+        rightRpm * reduction,
         CANSparkBase.ControlType.kVelocity,
         0,
         ff.calculate(rightRpm),
@@ -126,6 +126,7 @@ public void runCharacterizationRightVolts(double volts) {
 
   @Override
   public void stop() {
-    runVelocity(0.0, 0.0);
+    leftMotor.stopMotor();
+    rightMotor.stopMotor();
   }
 }