From 903ef58dafc13464b711184657bdbbc97bae766e Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Sun, 11 Feb 2024 19:24:43 -0500 Subject: [PATCH] More changes from WPI day 2 --- .../frc2024/RobotContainer.java | 14 +++++--- .../littletonrobotics/frc2024/RobotState.java | 2 +- .../frc2024/subsystems/drive/Drive.java | 36 ++++++++++++++++--- .../frc2024/subsystems/rollers/Rollers.java | 6 +++- .../subsystems/rollers/feeder/Feeder.java | 1 + .../subsystems/rollers/intake/Intake.java | 1 + .../superstructure/Superstructure.java | 30 +++------------- .../SuperstructureConstants.java | 2 +- .../superstructure/flywheels/Flywheels.java | 2 +- .../flywheels/FlywheelsIOSparkFlex.java | 7 ++-- 10 files changed, 59 insertions(+), 42 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 797d4181..1697a8dd 100644 --- 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 index 909a376a..5a08d693 100644 --- 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 index 0819613a..fd706ffb 100644 --- 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 index f76e6cec..d3e76f9c 100644 --- 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 index 4104bda4..c77326a5 100644 --- 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 index 29e734f4..e1451451 100644 --- 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 { 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 index d1ba1133..0cacc437 100644 --- 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/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java index c0de5346..e45dfa79 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java +++ b/src/main/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/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java index 826beb1d..0c48fe0d 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java +++ b/src/main/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/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSparkFlex.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSparkFlex.java index 942a231b..7a37950c 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSparkFlex.java +++ b/src/main/java/org/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(); } }