feat: Indexer Simulation

flamingchickens1540 · Jan 28, 2024 · 33b1517 · 33b1517
1 parent 61e7def
commit 33b1517
Show file tree

Hide file tree

Showing 5 changed files with 82 additions and 13 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -63,7 +63,8 @@ public static class Indexer {
         public static final double FEEDER_KD = 0.0;
         public static final double FEEDER_KS = 0.0;
         public static final double FEEDER_KV = 0.0;
-        public static final double GEAR_RATIO = 0.0;
+        public static final double FEEDER_GEAR_RATIO = 0.0;
+        public static final double INTAKE_GEAR_RATIO = 0.0;
 
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -12,7 +12,12 @@
 
 import org.team1540.robot2024.commands.FeedForwardCharacterization;
 import org.team1540.robot2024.commands.SwerveDriveCommand;
+import org.team1540.robot2024.commands.indexer.IntakeCommand;
 import org.team1540.robot2024.subsystems.drive.*;
+import org.team1540.robot2024.subsystems.indexer.Indexer;
+import org.team1540.robot2024.subsystems.indexer.IndexerIO;
+import org.team1540.robot2024.subsystems.indexer.IndexerIOSim;
+import org.team1540.robot2024.subsystems.indexer.IndexerIOSparkMax;
 import org.team1540.robot2024.util.swerve.SwerveFactory;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
@@ -31,6 +36,7 @@ public class RobotContainer {
     // Controller
     public final CommandXboxController driver = new CommandXboxController(0);
     public final CommandXboxController copilot = new CommandXboxController(1);
+    public final Indexer indexer;
 
     // Dashboard inputs
     public final LoggedDashboardChooser<Command> autoChooser;
@@ -49,6 +55,11 @@ public RobotContainer() {
                                 new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT)),
                                 new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT)),
                                 new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT)));
+
+                indexer =
+                        new Indexer(
+                                new IndexerIOSparkMax()
+                        );
                 break;
 
             case SIM:
@@ -60,6 +71,10 @@ public RobotContainer() {
                                 new ModuleIOSim(),
                                 new ModuleIOSim(),
                                 new ModuleIOSim());
+                indexer =
+                        new Indexer(
+                                new IndexerIOSim()
+                        );
                 break;
 
             default:
@@ -76,6 +91,10 @@ public RobotContainer() {
                                 },
                                 new ModuleIO() {
                                 });
+                indexer =
+                        new Indexer(
+                                new IndexerIO() {}
+                        );
                 break;
         }
 
@@ -108,6 +127,10 @@ private void configureButtonBindings() {
                         drivetrain
                 ).ignoringDisable(true)
         );
+
+        copilot.x().onTrue(indexer.feedToAmp());
+        copilot.y().onTrue(indexer.feedToShooter());
+        driver.a().onTrue(new IntakeCommand(indexer));
     }
 
     /**

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java
@@ -7,7 +7,7 @@ public interface IndexerIO {
     @AutoLog
     class IndexerIOInputs {
         public double intakeVoltage;
-        public double intakeCurrent;
+        public double intakeCurrentAmps;
         public double intakeVelocityRPM;
         public double feederVoltage;
         public double feederCurrentAmps;

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java
@@ -0,0 +1,51 @@
+package org.team1540.robot2024.subsystems.indexer;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
+import org.team1540.robot2024.Constants;
+
+import static org.team1540.robot2024.Constants.Indexer.*;
+
+public class IndexerIOSim implements IndexerIO {
+
+
+    private final DCMotorSim intakeSim = new DCMotorSim(DCMotor.getNEO(1), INTAKE_GEAR_RATIO,0.025);
+    private final DCMotorSim feederSim = new DCMotorSim(DCMotor.getNEO(1), FEEDER_GEAR_RATIO,0.025);
+    private final SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break");
+    private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI,FEEDER_KD);
+
+    @Override
+    public void updateInputs(IndexerIOInputs inputs) {
+        intakeSim.update(Constants.LOOP_PERIOD_SECS);
+        feederSim.update(Constants.LOOP_PERIOD_SECS);
+        inputs.intakeCurrentAmps = intakeSim.getCurrentDrawAmps();
+//        inputs.intakeVoltage = intakeSim.getBusVoltage() * intakeSim.getAppliedOutput();
+        inputs.intakeVelocityRPM = intakeSim.getAngularVelocityRPM();
+        inputs.feederCurrentAmps = feederSim.getCurrentDrawAmps();
+//        inputs.feederVoltage = feederSim.getBusVoltage() * feederSim.getAppliedOutput();
+        inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM();
+        inputs.noteInIntake = beamBreakSim.getBoolean("Indexer Beam Break").get();
+
+        // this is a very funny line of code, and absolutely does not belong here, but I don't know how to do this otherwise
+        feederSim.setState(feederSim.getAngularPositionRad(), feederSimPID.calculate(feederSim.getAngularVelocityRadPerSec()));
+
+    }
+
+    @Override
+    public void setIntakeVoltage(double volts) {
+        intakeSim.setInputVoltage(MathUtil.clamp(volts, -12, 12));
+    }
+
+    @Override
+    public void setFeederVelocity(double velocity) {
+        feederSimPID.setSetpoint(velocity);
+    }
+
+    @Override
+    public void setFeederVoltage(double volts) {
+        feederSim.setInputVoltage(MathUtil.clamp(volts, -12, 12));
+    }
+}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java
@@ -11,7 +11,7 @@
 public class IndexerIOSparkMax implements IndexerIO {
     private final CANSparkMax intakeMotor = new CANSparkMax(INTAKE_ID, CANSparkLowLevel.MotorType.kBrushless);
     private final CANSparkMax feederMotor = new CANSparkMax(FEEDER_ID, CANSparkLowLevel.MotorType.kBrushless);
-    DigitalInput indexerBeamBreak = new DigitalInput(0);
+    private final DigitalInput indexerBeamBreak = new DigitalInput(0);
     private final SparkPIDController feederPID;
     private final SimpleMotorFeedforward feederFF = new SimpleMotorFeedforward(FEEDER_KS, FEEDER_KV);
 
@@ -33,19 +33,14 @@ public IndexerIOSparkMax() {
 
 
     }
-    // 2 jobs,
-    // 1. be able to route to tramp
-    // 2. feeding the shooter (by default, be still, spin up fast 4 for shooter.
-    // make cmd factory spin
-    // slower in other direction for tramp
 
     @Override
     public void updateInputs(IndexerIOInputs inputs) {
-        inputs.intakeCurrent = intakeMotor.getOutputCurrent();
+        inputs.intakeCurrentAmps = intakeMotor.getOutputCurrent();
         inputs.intakeVoltage = intakeMotor.getBusVoltage() * intakeMotor.getAppliedOutput();
         inputs.intakeVelocityRPM = intakeMotor.getEncoder().getVelocity();
-        inputs.feederVoltage = feederMotor.getOutputCurrent();
-        inputs.feederCurrentAmps = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput();
+        inputs.feederCurrentAmps = feederMotor.getOutputCurrent();
+        inputs.feederVoltage = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput();
         inputs.feederVelocityRPM = feederMotor.getEncoder().getVelocity();
         inputs.noteInIntake = indexerBeamBreak.get();
     }
@@ -63,12 +58,11 @@ public void setFeederVoltage(double volts) {
     @Override
     public void setFeederVelocity(double velocity) {
         feederPID.setReference(
-                Units.radiansPerSecondToRotationsPerMinute(velocity) * GEAR_RATIO,
+                Units.radiansPerSecondToRotationsPerMinute(velocity) * FEEDER_GEAR_RATIO,
                 CANSparkBase.ControlType.kVelocity,
                 0,
                 feederFF.calculate(velocity),
                 SparkPIDController.ArbFFUnits.kVoltage
         );
     }
 }
-// for elevator sim, app. voltage