feat: shooter tuning and indexer tuning

src/main/java/org/team1540/bunnybotTank2023/Constants.java

@@ -63,11 +63,11 @@ public static class ShooterConstants {
         public static final int LEADER_ID = 20;
         public static final int FOLLOWER_ID = 21;
 
-        public static final double KP = 0.6; // TODO: 11/30/2023 tuned, doesn't work well for values < 1000 rpm
-        public static final double KI = 0.07;
+        public static final double KP = 0.4; // TODO: 11/30/2023 tuned, doesn't work well for values < 1000 rpm
+        public static final double KI = 0.05;
         public static final double KD = 0;
-        public static final double KS = 0.19;
-        public static final double KV = 0.1149;
+        public static final double KS = 0;
+        public static final double KV = 0.113;
 
         public static final double ERROR_TOLERANCE_RPM = 30;
         public static final double SHOOTER_IDLE_RPM = 1000;

src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java

@@ -6,14 +6,20 @@
 package org.team1540.bunnybotTank2023;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.StartEndCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import org.littletonrobotics.junction.networktables.LoggedDashboardInput;
+import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 import org.team1540.bunnybotTank2023.commands.auto.AutoShoot5RamTotes;
 import org.team1540.bunnybotTank2023.commands.drivetrain.Drivetrain;
 import org.team1540.bunnybotTank2023.commands.drivetrain.TankdriveCommand;
+import org.team1540.bunnybotTank2023.commands.indexer.Indexer;
+import org.team1540.bunnybotTank2023.commands.indexer.IndexerCommand;
 import org.team1540.bunnybotTank2023.commands.shooter.Shooter;
 import org.team1540.bunnybotTank2023.io.drivetrain.DrivetrainIOSim;
 import org.team1540.bunnybotTank2023.io.drivetrain.DrivetrainIOReal;
+import org.team1540.bunnybotTank2023.io.indexer.IndexerIOReal;
 import org.team1540.bunnybotTank2023.io.shooter.ShooterIOReal;
 import org.team1540.bunnybotTank2023.io.shooter.ShooterIOSim;
 
@@ -29,20 +35,25 @@ public class RobotContainer {
     // Subsystems
     Drivetrain drivetrain;
     Shooter shooter;
+    Indexer indexer;
 
     // Controllers
     CommandXboxController driver = new CommandXboxController(0);
     CommandXboxController copilot = new CommandXboxController(1);
 
+    LoggedDashboardNumber shooterSpeed = new LoggedDashboardNumber("Shooter/setpoint");
+
     public RobotContainer() {
         if (Robot.isReal()) {
             // Initialize subsystems with hardware IO
             drivetrain = new Drivetrain(new DrivetrainIOReal());
             shooter = new Shooter(new ShooterIOReal());
+            indexer = new Indexer(new IndexerIOReal());
         } else {
             // Initialize subsystems with simulation IO
             drivetrain = new Drivetrain(new DrivetrainIOSim());
             shooter = new Shooter(new ShooterIOSim());
+            indexer = null;
         }
         setDefaultCommands();
         configureButtonBindings();
@@ -51,13 +62,22 @@ public RobotContainer() {
 
     /** Use this method to define your trigger->command mappings. */
     private void configureButtonBindings() {
+        driver.a()
+                .whileTrue(new InstantCommand(() -> shooter.setVelocity(shooterSpeed.get()))
+                        .andThen(new IndexerCommand(indexer)))
+                .onFalse(new InstantCommand(() -> {
+                    shooter.stop();
+                    indexer.stop();
+                }));
+
+        driver.b().whileTrue(new IndexerCommand(indexer));
     }
 
     private void setDefaultCommands() {
-        drivetrain.setDefaultCommand(new TankdriveCommand(drivetrain, driver));
+//        drivetrain.setDefaultCommand(new TankdriveCommand(drivetrain, driver));
         shooter.setDefaultCommand(new StartEndCommand(
                 () -> shooter.setVelocity(ShooterConstants.SHOOTER_IDLE_RPM),
-                () -> shooter.stop(),
+                () -> {},
                 shooter
         ));
     }

src/main/java/org/team1540/bunnybotTank2023/commands/indexer/Indexer.java

@@ -26,4 +26,9 @@ public void setTopSpeed(double speed) {
     public void setBottomSpeed(double speed) {
         io.setBottomPercent(speed);
     }
+
+    public void stop() {
+        setTopSpeed(0);
+        setBottomSpeed(0);
+    }
 }

src/main/java/org/team1540/bunnybotTank2023/commands/indexer/IndexerCommand.java

@@ -0,0 +1,22 @@
+package org.team1540.bunnybotTank2023.commands.indexer;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class IndexerCommand extends CommandBase {
+    private final Indexer indexer;
+
+    public IndexerCommand(Indexer indexer) {
+        this.indexer = indexer;
+    }
+
+    @Override
+    public void initialize() {
+        indexer.setBottomSpeed(0.75);
+        indexer.setTopSpeed(0.75);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        indexer.stop();
+    }
+}

src/main/java/org/team1540/bunnybotTank2023/commands/shooter/Shooter.java

@@ -41,6 +41,7 @@ public void periodic() {
     }
 
     public void setVelocity(double speedRPM) {
+        System.out.println("test");
         setpoint = speedRPM;
         averageFilter.clear();
         io.setVelocity(speedRPM);

src/main/java/org/team1540/bunnybotTank2023/io/indexer/IndexerIOReal.java

@@ -15,12 +15,12 @@ public IndexerIOReal() {
 
         topMotor1.setSmartCurrentLimit(30);
         topMotor2.setSmartCurrentLimit(30);
-        bottomMotor.setSmartCurrentLimit(30);
+        bottomMotor.setSmartCurrentLimit(25);
 
-        topMotor2.follow(topMotor1);
+        topMotor2.follow(topMotor1, true);
 
         topMotor1.setInverted(false);
-        topMotor2.setInverted(true);
+//        topMotor2.setInverted(false);
         bottomMotor.setInverted(true);
     }