update autorework (#56)

* feat: update swerve config for krakens and adjusted inversion (#53) * fix: dio ports * feat: use new beam breaks * feat: updated drivetrain constants --------- Co-authored-by: Zach Rutman <zarutman+git@gmail.com> Co-authored-by: Simon Weil <113215817+WeilSimon@users.noreply.github.com>
flamingchickens1540 · Sep 23, 2024 · 377ceba · 377ceba
1 parent 326ff0e
commit 377ceba
Show file tree

Hide file tree

Showing 13 changed files with 66 additions and 118 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -19,6 +19,12 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
+    public static final class DIO {
+        public static final int INTAKE_BEAM_BREAK = 6;
+        public static final int SHOOTER_BEAM_BREAK = 4;
+        public static final int INDEXER_BEAM_BREAK = 5;
+        public static final int TRAMP_BEAM_BREAK = 9;
+    }
     public static final boolean IS_COMPETITION_ROBOT = true;
     // Whether to pull PID constants from SmartDashboard
     private static final boolean tuningMode = true; // TODO: DO NOT SET TO TRUE FOR COMP
@@ -61,11 +67,11 @@ public static class SwerveConfig {
     }
 
     public static class Drivetrain {
-        public static final boolean IS_L3 = !IS_COMPETITION_ROBOT;
+        public static final boolean IS_L3 = true;
         public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (IS_L3 ? 16.0 / 28.0 : 17.0 / 27.0) * (45.0 / 15.0);
         public static final double TURN_GEAR_RATIO = 150.0 / 7.0;
         public static final boolean IS_TURN_MOTOR_INVERTED = true;
-        public static final double WHEEL_RADIUS = Units.inchesToMeters(1.9448479168443666);
+        public static final double WHEEL_RADIUS = Units.inchesToMeters(1.9836954390238841);
 
         public static final double MAX_LINEAR_SPEED = Units.feetToMeters(IS_L3 ? 16.0 : 15.7);
         public static final double TRACK_WIDTH_X = Units.inchesToMeters(18.75);
@@ -89,7 +95,6 @@ public static class Auto {
     }
 
     public static class Indexer {
-        public static final int BEAM_BREAK_ID = IS_COMPETITION_ROBOT ? 7 : 8;
         public static final int INTAKE_ID = 13;
         public static final int FEEDER_ID = 15;
 
@@ -258,7 +263,6 @@ public enum ElevatorState {
     }
 
     public static class Tramp {
-        public static final int BEAM_BREAK_CHANNEL = 9;
         public static final double GEAR_RATIO = 9.0;
         public static final int MOTOR_ID = 17;
     }

diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java
@@ -4,6 +4,7 @@
 import com.pathplanner.lib.commands.FollowPathCommand;
 import com.pathplanner.lib.commands.PathfindingCommand;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -122,7 +123,6 @@ public void robotPeriodic() {
         // the Command-based framework to work.
         CommandScheduler.getInstance().run();
         if (Constants.currentMode == Constants.Mode.REAL) robotContainer.odometrySignalRefresher.periodic();
-
         // Update mechanism visualiser in sim
         if (Robot.isSimulation()) MechanismVisualiser.periodic();
 //        robotContainer.leds.setPattern(Leds.Zone.ZONE1, SimpleLedPattern.alternating(Color.kBlueViolet, Color.kGreen));

diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -15,6 +15,7 @@
 import org.team1540.robot2024.commands.climb.ClimbAlignment;
 import org.team1540.robot2024.commands.drivetrain.*;
 import org.team1540.robot2024.commands.elevator.ElevatorManualCommand;
+import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand;
 import org.team1540.robot2024.commands.indexer.ContinuousIntakeCommand;
 import org.team1540.robot2024.commands.indexer.IntakeAndFeed;
 import org.team1540.robot2024.commands.indexer.StageTrampCommand;
@@ -148,6 +149,7 @@ private void configureButtonBindings() {
                 .alongWith(leds.commandShowPattern(
                         new LedPatternProgressBar(shooter::getSpinUpPercent, "#00ffbc", 33),
                         Leds.PatternLevel.DRIVER_LOCK));
+
         Command ampLock = new DriveWithAmpSideLock(drivetrain, driver.getHID())
                 .alongWith(leds.commandShowPattern(new LedPatternWave("#ffffff"), Leds.PatternLevel.DRIVER_LOCK));
         Command cancelAlignment = Commands.runOnce(() -> {
@@ -191,6 +193,10 @@ private void configureButtonBindings() {
         copilot.povUp().whileTrue(indexer.commandRunIntake(1));
         copilot.povRight().whileTrue(IntakeAndFeed.withDefaults(indexer)).onFalse(cancelAlignment);
         copilot.povLeft().onTrue(Commands.runOnce(()->elevator.setFlipper(true))).onFalse(Commands.runOnce(()->elevator.setFlipper(false)));
+//        copilot.povDown().whileTrue(new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.BOTTOM));
+//        copilot.povUp().whileTrue(new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.TOP));
+//        copilot.povRight().whileTrue(new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.CLIMB));
+//        copilot.povLeft().whileTrue(new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.AMP));
 
 
         copilot.rightTrigger(0.95).whileTrue(tramp.commandRun(1));

diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java
@@ -35,17 +35,14 @@ public void initialize() {
 
     @Override
     public void execute() {
-        if (indexer.isNoteStaged()) {
+        if (indexer.getNoteState() == Indexer.NotePosition.INDEXER) {
             indexer.stopIntake();
             if (leds != null) {
                 leds.clearPatternAll(Leds.PatternLevel.INTAKE_PREREADY);
             }
         } else {
             if (leds != null) {
-                if (indexer.getIntakeVoltage() == 0) {
-                    timer.restart();
-                }
-                if (timer.hasElapsed(0.3) && indexer.getIntakeCurrent() > 30) {
+                if (indexer.getNoteState() == Indexer.NotePosition.INTAKE) {
                     leds.setPatternAll(detected, Leds.PatternLevel.INTAKE_PREREADY);
                 }
             }

diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java
@@ -32,7 +32,7 @@ public Module(ModuleIO io, int index) {
         switch (Constants.currentMode) {
             case REAL:
             case REPLAY:
-                driveFeedforward = new SimpleMotorFeedforward(0.258, 0.128);
+                driveFeedforward = new SimpleMotorFeedforward(0.22906946972684633, 0.12468104575529199);
 //                driveFeedforward = new SimpleMotorFeedforward(0.206, 0.117);
                 driveFeedback = new PIDController(0.05, 0.0, 0.0);
                 turnFeedback = new PIDController(7.0, 0.0, 0.0);

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java
@@ -4,12 +4,12 @@
 import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 import org.team1540.robot2024.Constants;
-import org.team1540.robot2024.util.LoggedTunableNumber;
+
+import java.util.function.BooleanSupplier;
 
 import static org.team1540.robot2024.Constants.Indexer.*;
 
@@ -18,10 +18,6 @@ public class Indexer extends SubsystemBase {
     private final IndexerIO io;
     private final IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged();
 
-    private final LoggedTunableNumber kP = new LoggedTunableNumber("Indexer/kP", FEEDER_KP);
-    private final LoggedTunableNumber kI = new LoggedTunableNumber("Indexer/kI", FEEDER_KI);
-    private final LoggedTunableNumber kD = new LoggedTunableNumber("Indexer/kD", FEEDER_KD);
-
     private double feederSetpointRPM = 0.0;
 
     private static boolean hasInstance = false;
@@ -68,7 +64,23 @@ public void setIntakePercent(double percent) {
     }
 
     public boolean isNoteStaged() {
-        return inputs.noteInIntake;
+        return inputs.noteInIndexer;
+    }
+
+    public BooleanSupplier checkNoteReached(NotePosition position) {
+        return () -> getNoteState().ordinal() >= position.ordinal();
+    }
+    public NotePosition getNoteState() {
+        if (inputs.noteInShooter) {
+            return NotePosition.SHOOTER;
+        }
+        if (inputs.noteInIndexer) {
+            return NotePosition.INDEXER;
+        }
+        if (inputs.noteInIntake) {
+            return NotePosition.INTAKE;
+        }
+        return NotePosition.NONE;
     }
 
     public void setFeederVelocity(double setpointRPM) {
@@ -114,16 +126,6 @@ public Command feedToAmp() {
     public Command feedToShooter() {
         return Commands.runOnce(() -> io.setFeederVelocity(1200), this);
     }
-
-    public Command moveNoteOut() {
-        return new FunctionalCommand(
-                () -> setIntakePercent(-1),
-                () -> {},
-                (interrupted) -> stopIntake(),
-                () -> !isNoteStaged(),
-                this
-        );
-    }
 
     public Command commandRunIntake(double percent) {
         return Commands.startEnd(
@@ -133,6 +135,13 @@ public Command commandRunIntake(double percent) {
                 );
     }
 
+    public Command moveNoteTo(NotePosition position) {
+        return Commands.startEnd(() -> {
+            this.setIntakePercent(1);
+            this.setFeederPercent(1);
+        }, this::stopAll).until(this.checkNoteReached(NotePosition.SHOOTER));
+    }
+
     public void setIntakeBrakeMode(boolean isBrake) {
         io.setIntakeBrakeMode(isBrake);
     }
@@ -145,4 +154,10 @@ public double getIntakeCurrent() {
         return inputs.intakeCurrentAmps;
     }
 
+    public enum NotePosition {
+        NONE,
+        INTAKE,
+        INDEXER,
+        SHOOTER;
+    }
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java
@@ -14,7 +14,9 @@ class IndexerIOInputs {
         public double feederCurrentAmps = 0.0;
         public double feederVelocityRPM = 0.0;
         public double feederTempCelsius = 0.0;
+        public boolean noteInIndexer = false;
         public boolean noteInIntake = false;
+        public boolean noteInShooter = false;
     }
 
     /**

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java
@@ -15,7 +15,7 @@ public class IndexerIOSim implements IndexerIO {
     private final DCMotorSim intakeSim = new DCMotorSim(DCMotor.getNEO(1), INTAKE_GEAR_RATIO, INTAKE_MOI);
     private final DCMotorSim feederSim = new DCMotorSim(DCMotor.getNEO(1), FEEDER_GEAR_RATIO, FEEDER_MOI);
     //    private final SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break");
-    private final DigitalInput indexerBeamBreak = new DigitalInput(BEAM_BREAK_ID);
+    private final DigitalInput indexerBeamBreak = new DigitalInput(Constants.DIO.INDEXER_BEAM_BREAK);
     private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI, FEEDER_KD);
     private boolean isClosedLoop = true;
     private double intakeVoltage = 0.0;
@@ -39,7 +39,7 @@ public void updateInputs(IndexerIOInputs inputs) {
         inputs.feederCurrentAmps = feederSim.getCurrentDrawAmps();
         inputs.feederVoltage = feederVoltage;
         inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM();
-        inputs.noteInIntake = indexerBeamBreak.get();
+        inputs.noteInIndexer = indexerBeamBreak.get();
     }
 
     @Override

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java
diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java
@@ -10,13 +10,16 @@
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.wpilibj.DigitalInput;
+import org.team1540.robot2024.Constants;
 
 import static org.team1540.robot2024.Constants.Indexer.*;
 
 public class IndexerIOTalonFX implements IndexerIO {
     private final TalonFX intakeMotor = new TalonFX(INTAKE_ID);
     private final TalonFX feederMotor = new TalonFX(FEEDER_ID);
-    private final DigitalInput indexerBeamBreak = new DigitalInput(BEAM_BREAK_ID);
+    private final DigitalInput indexerBeamBreak = new DigitalInput(Constants.DIO.INDEXER_BEAM_BREAK);
+    private final DigitalInput intakeBeamBreak = new DigitalInput(Constants.DIO.INTAKE_BEAM_BREAK);
+    private final DigitalInput shooterBeamBreak = new DigitalInput(Constants.DIO.SHOOTER_BEAM_BREAK);
 
     private final VoltageOut feederVoltageCtrlReq = new VoltageOut(0).withEnableFOC(true);
     private final VelocityVoltage feederVelocityCtrlReq = new VelocityVoltage(0).withEnableFOC(true);
@@ -87,7 +90,9 @@ public void updateInputs(IndexerIOInputs inputs) {
         inputs.feederCurrentAmps = feederCurrent.getValueAsDouble();
         inputs.feederVelocityRPM = feederVelocity.getValueAsDouble() * 60;
         inputs.feederTempCelsius = feederTemp.getValueAsDouble();
-        inputs.noteInIntake = !indexerBeamBreak.get();
+        inputs.noteInIndexer = !indexerBeamBreak.get();
+        inputs.noteInIntake = !intakeBeamBreak.get();
+        inputs.noteInShooter = !shooterBeamBreak.get();
     }
 
     @Override

diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSim.java
@@ -6,12 +6,11 @@
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import org.team1540.robot2024.Constants;
 
-import static org.team1540.robot2024.Constants.Tramp.BEAM_BREAK_CHANNEL;
 import static org.team1540.robot2024.Constants.Tramp.GEAR_RATIO;
 
 public class TrampIOSim implements TrampIO {
     private final DCMotorSim sim = new DCMotorSim(DCMotor.getNEO(1), GEAR_RATIO, 0.025); //TODO: Fix MOI its probably wrong :D
-    private final DigitalInput beamBreak = new DigitalInput(BEAM_BREAK_CHANNEL);
+    private final DigitalInput beamBreak = new DigitalInput(Constants.DIO.TRAMP_BEAM_BREAK);
     private double appliedVolts = 0.0;
 
 

diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java
@@ -4,11 +4,12 @@
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import edu.wpi.first.wpilibj.DigitalInput;
+import org.team1540.robot2024.Constants;
 
 import static org.team1540.robot2024.Constants.Tramp.*;
 
 public class TrampIOSparkMax implements TrampIO {
-    private final DigitalInput beamBreak = new DigitalInput(BEAM_BREAK_CHANNEL);
+    private final DigitalInput beamBreak = new DigitalInput(Constants.DIO.TRAMP_BEAM_BREAK);
     private final CANSparkMax motor = new CANSparkMax(MOTOR_ID, MotorType.kBrushless);
     private final RelativeEncoder motorEncoder = motor.getEncoder();
 

diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOTalonFX.java
@@ -7,12 +7,12 @@
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.wpilibj.DigitalInput;
+import org.team1540.robot2024.Constants;
 
-import static org.team1540.robot2024.Constants.Tramp.BEAM_BREAK_CHANNEL;
 import static org.team1540.robot2024.Constants.Tramp.MOTOR_ID;
 
 public class TrampIOTalonFX implements TrampIO {
-    private final DigitalInput beamBreak = new DigitalInput(BEAM_BREAK_CHANNEL);
+    private final DigitalInput beamBreak = new DigitalInput(Constants.DIO.TRAMP_BEAM_BREAK);
     private final TalonFX motor = new TalonFX(MOTOR_ID);
     private final StatusSignal<Double> velocity = motor.getVelocity();
     private final StatusSignal<Double> position = motor.getPosition();