PID Ekansh

src/main/java/frc/robot/Orchestrator.java

@@ -1,27 +1,48 @@
 package frc.robot;
 
 import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.ShooterConstants;
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
 
 
 public class Orchestrator {
     private final Indexer indexer;
     private final Shooter shooter;
+    @AutoLogOutput
+    private double Distance  = 300.0;
+    @AutoLogOutput private double shootingPower = 0.0;
 
     public Orchestrator(Indexer indexer, Shooter shooter) {
         this.indexer = indexer;
         this.shooter = shooter;
     }
+    public Command spinUpDistance(Double distance) {
+        Distance = distance;
+        double percent = (8.331 * Math.pow(10, -5) * Math.pow(distance, 2) + 0.0734 * distance + 15.709) / 100;
+        Logger.recordOutput("Shooter/Percent", percent);
+        return shooter.runPercent(percent);
+    }
+    public Command shootCycleDistance() {
+        return Commands.parallel(
+                spinUpDistance(420.0),
+                Commands.sequence(
+                                intakeIfNeeded(),
+                                Commands.waitSeconds(ShooterConstants.SPINUP_SECONDS),
+                                indexer.indexIntoShooter())
+                        .repeatedly());
+    }
 
     public Command spinUp(BooleanSupplier fastMode) {
         return Commands.either(
                 shooter.fullSpeed(),
-                shooter.speedUp(),
+                shooter.runPercent(0.8),
                  fastMode);
     }
 
@@ -51,6 +72,21 @@ public Command shootCycle(BooleanSupplier fastMode) {
                         .repeatedly());
     }
 
+    public Command shootOnceSetpoint(DoubleSupplier setpoint) {
+        return Commands.parallel(
+                shooter.toSetpoint(setpoint)
+                        .withTimeout(0.001)
+                        .andThen(shooter.toSetpoint(setpoint).until(shooter::atSetpoint)),
+                Commands.sequence(
+                        intakeIfNeeded(),
+                        Commands.waitSeconds(ShooterConstants.SPINUP_SECONDS),
+                        indexer.indexIntoShooter()));
+    }
+
+    public Command shootCycleSetpoint(DoubleSupplier setpoint) {
+        return shootOnceSetpoint(setpoint).repeatedly();
+    }
+
     public Command reverseAll() {
         return Commands.parallel(shooter.reverse(), indexer.reverse());
     }

src/main/java/frc/robot/RobotContainer.java

@@ -17,7 +17,6 @@
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.ShooterIOSparkMax;
 import frc.robot.subsystems.vision.Vision;
-import frc.robot.subsystems.vision.VisionIO;
 import frc.robot.subsystems.vision.VisionIOPhotonVision;
 
 
@@ -31,7 +30,6 @@ public class RobotContainer {
   // The robot's subsystems and commands are defined here...
   private final Drive drive;
     // The robot's subsystems and commands are defined here...
-    private final Vision vision;
    private final Indexer indexer = new Indexer(new IndexerIOSparkMax());
     private final Shooter shooter = new Shooter(new ShooterIOSparkMax());
     private final Orchestrator orchestrator = new Orchestrator(indexer, shooter);
@@ -44,7 +42,6 @@ public class RobotContainer {
   public RobotContainer() {
     // Configure the trigger bindings
     drive = new Drive(new DriveIOSparkMax());
-    vision =  new Vision(new VisionIOPhotonVision("VGA_USB_Camera"){});
     configureBindings();
   }
 
@@ -75,16 +72,36 @@ private void configureBindings() {
                       .rightTrigger()
                       .whileTrue(orchestrator.shootCycle(() -> fastMode))
                       .onFalse(orchestrator.stopAll());
-              driverController.y().whileTrue(shooter.speedUp()).onFalse(shooter.stop());
-              driverController
-                      .leftTrigger()
-                      .whileTrue(orchestrator.reverseAll())
-                      .onFalse(orchestrator.stopAll());
-
+
               driverController
                       .rightBumper()
                       .onTrue(orchestrator.shootOnce(() -> fastMode))
                       .onFalse(orchestrator.stopAll());
+
+              driverController
+                      .b()
+                      .whileTrue(orchestrator.shootCycleSetpoint(() -> 0.5))
+                      .onFalse(orchestrator.stopAll());
+
+              driverController
+                      .b()
+                      .onTrue(orchestrator.shootOnceSetpoint(() -> 0.5))
+                      .onFalse(orchestrator.stopAll());
+
+              driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop());
+
+              driverController.leftTrigger().onTrue(indexer.indexUntilSwitch());
+
+              driverController.y().onTrue(indexer.indexIntoShooter());
+
+
+              driverController
+                      .leftBumper()
+                      .whileTrue(orchestrator.reverseAll())
+                      .onFalse(orchestrator.stopAll());
+
+
+
     }
 
     /**

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -15,11 +15,13 @@ public class Shooter extends SubsystemBase {
     public Shooter(ShooterIO io) {
         this.io = io;
         this.inputs = new ShooterIOInputsAutoLogged();
+        this.io.setSpeed(0.5); // TODO: Change the random value
     }
 
     @Override
     public void periodic() {
         io.updateInputs(inputs);
+        io.goToSetpoint();
         Logger.processInputs("Shooter", inputs);
     }
 
@@ -36,24 +38,39 @@ public Command reverse() {
          );
 }
 
-
-    public Command speedUp() {
+    public Command fullSpeed() {
         return Commands.run(
                 () -> {
-                    io.setPercent(ShooterConstants.SPEED);
+                    io.setPercent(1);
                 },
                 this
         );
     }
 
-    public Command fullSpeed() {
+    public Command runPercent(double percent){
+        return Commands.run(() -> {
+            io.setPercent(percent);
+        },
+                this);
+    }
+
+    public boolean atSetpoint() {
+        return inputs.atSetpoint;
+    }
+
+    public Command toSetpoint(double setpointPercent) {
         return Commands.run(
                 () -> {
-                    io.setPercent(1);
+                    io.setSpeed(setpointPercent);
                 },
-                this
-        );
+                this);
     }
 
-
+    public Command toSetpoint(DoubleSupplier setpointPercent) {
+        return Commands.run(
+                () -> {
+                    io.setSpeed(setpointPercent.getAsDouble());
+                },
+                this);
+    }
 }

src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java

@@ -2,8 +2,6 @@
 
 public class ShooterConstants {
 
-    public static final double SPEED = 0.8;
-
     public static final double REVERSE_SHOOTER_PERCENT = -0.5;
 
     public static final int SHOOTER_MOTOR_ID = 8;
@@ -12,5 +10,11 @@ public class ShooterConstants {
 
     public static final double ENCODER_VELOCITY_CONVERSION = 1.0;
 
-    public static final double SPINUP_SECONDS = 0.1;
+    public static final double SPINUP_SECONDS = 0.6;
+
+    public static final double TOLERANCE = 0.1;
+
+    public static final double PID_P = 15.0;
+
+    public static final double PID_D = 7.0;
 }

src/main/java/frc/robot/subsystems/shooter/ShooterIO.java

@@ -16,14 +16,23 @@ class ShooterIOInputs {
 
         public double temperature;
 
+        public double angularVelocity;
+
         public boolean readyToShoot = false;
+
+        public boolean atSetpoint = false;
     }
 
+    default void setSpeed(double setpoint) {}
+
     default void updateInputs(ShooterIOInputs inputs) {}
 
     default void setPercent(double percent) {}
 
     default void setVoltage(double voltage) {}
 
     default void stop() {}
+
+    default void goToSetpoint() {}
+
 }

src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java

@@ -3,11 +3,13 @@
 import static frc.robot.subsystems.shooter.ShooterConstants.*;
 
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkBase;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.SparkLimitSwitch;
+import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.ClosedLoopConfig;
 import com.revrobotics.spark.config.EncoderConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
@@ -17,14 +19,24 @@ public class ShooterIOSparkMax implements ShooterIO {
     private final SparkMax motor;
     private final RelativeEncoder encoder;
 
+    private final SparkClosedLoopController controller;
+    double setpoint = 0.0;
+
     public ShooterIOSparkMax() {
         motor = new SparkMax(SHOOTER_MOTOR_ID, MotorType.kBrushless);
 
+        controller = motor.getClosedLoopController();
+
         var config = new SparkMaxConfig();
         config.inverted(false)
                 .idleMode(IdleMode.kBrake)
                 .voltageCompensation(12)
                 .smartCurrentLimit(30);
+        config
+                .closedLoop
+                .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder)
+                .positionWrappingEnabled(false)
+                .pidf(PID_P, 0.0, PID_D, 0.0);
 
         EncoderConfig enc = new EncoderConfig();
         enc.positionConversionFactor(ENCODER_POSITION_CONVERSION);
@@ -43,6 +55,13 @@ public void updateInputs(ShooterIOInputs inputs) {
         inputs.appliedVolts = motor.getBusVoltage() * motor.getAppliedOutput();
         inputs.currentAmps = motor.getOutputCurrent();
         inputs.velocity = encoder.getVelocity();
+        inputs.angularVelocity = inputs.velocity * (2 * Math.PI / 60);
+        inputs.atSetpoint = Math.abs(setpoint - inputs.velocity) < TOLERANCE;
+    }
+
+    @Override
+    public void setSpeed(double setpoint) {
+        this.setpoint = setpoint;
     }
 
     public void setPercent(double percent) {
@@ -56,4 +75,9 @@ public void setVoltage(double volts) {
     public void stop() {
         motor.set(0);
     }
+
+    @Override
+    public void goToSetpoint() {
+        controller.setReference(this.setpoint, SparkBase.ControlType.kVelocity);
+    }
 }