PID Ronit

.gitignore

@@ -28,7 +28,7 @@
 *.lai
 *.la
 *.a
-*.lib
+*.frc.robot.lib
 
 # Executables
 *.exe

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

@@ -7,22 +7,42 @@
 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(),
-                 fastMode);
+                shooter.runPercent(0.8),
+                fastMode);
     }
 
     public Command intakeIfNeeded() {
@@ -58,4 +78,4 @@ public Command reverseAll() {
     public Command stopAll() {
         return Commands.parallel(shooter.stop(), indexer.stop());
     }
-}
+}

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,26 @@ 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.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

@@ -37,15 +37,12 @@ public Command reverse() {
 }
 
 
-    public Command speedUp() {
+    public Command toVelocity(double velocityRPM) {
         return Commands.run(
-                () -> {
-                    io.setPercent(ShooterConstants.SPEED);
-                },
+                () -> io.setVelocity(velocityRPM),
                 this
         );
     }
-
     public Command fullSpeed() {
         return Commands.run(
                 () -> {
@@ -54,6 +51,11 @@ public Command fullSpeed() {
                 this
         );
     }
-
+        public Command runPercent(double percent){
+            return Commands.run(() -> {
+                io.setPercent(percent);
+            },
+                    this);
+            }
 
 }

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

@@ -2,15 +2,11 @@
 
 public class ShooterConstants {
 
-    public static final double SPEED = 0.8;
-
+    public static final double TOLERANCE = 0.0;
+    public static final double ENCODER_POSITION_CONVERSION_FACTOR = 1.0;
+    public static final double ENCODER_VELOCITY_CONVERSION_FACTOR = 1.0;
     public static final double REVERSE_SHOOTER_PERCENT = -0.5;
-
     public static final int SHOOTER_MOTOR_ID = 8;
+    public static final double SPINUP_SECONDS = 0.6;
 
-    public static final double ENCODER_POSITION_CONVERSION = 1.0;
-
-    public static final double ENCODER_VELOCITY_CONVERSION = 1.0;
-
-    public static final double SPINUP_SECONDS = 0.1;
 }

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

@@ -1,28 +1,26 @@
 package frc.robot.subsystems.shooter;
 
-import org.ejml.dense.row.factory.DecompositionFactory_CDRM;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface ShooterIO {
 
     @AutoLog
     class ShooterIOInputs {
-
-        public double velocity;
-
+        public double velocityRPM;
         public double appliedVolts;
-
         public double currentAmps;
-
         public double temperature;
-
+        public double targetVelocityRPM = 0.0;
+        public boolean atTargetVelocity;
         public boolean readyToShoot = false;
     }
 
     default void updateInputs(ShooterIOInputs inputs) {}
 
     default void setPercent(double percent) {}
 
+    default void setVelocity(double velocityRPM) {}
+
     default void setVoltage(double voltage) {}
 
     default void stop() {}

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

@@ -3,57 +3,87 @@
 import static frc.robot.subsystems.shooter.ShooterConstants.*;
 
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.*;
+import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.SparkLimitSwitch;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.config.EncoderConfig;
+import com.revrobotics.spark.config.ClosedLoopConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 public class ShooterIOSparkMax implements ShooterIO {
 
     private final SparkMax motor;
     private final RelativeEncoder encoder;
+    private final SparkClosedLoopController controller;
+
+    private double targetVelocityRPM = 0.0;
 
     public ShooterIOSparkMax() {
         motor = new SparkMax(SHOOTER_MOTOR_ID, MotorType.kBrushless);
+        encoder = motor.getEncoder();
+        controller = motor.getClosedLoopController();
 
         var config = new SparkMaxConfig();
         config.inverted(false)
-                .idleMode(IdleMode.kBrake)
+                .idleMode(IdleMode.kCoast)
                 .voltageCompensation(12)
                 .smartCurrentLimit(30);
 
-        EncoderConfig enc = new EncoderConfig();
-        enc.positionConversionFactor(ENCODER_POSITION_CONVERSION);
-        enc.velocityConversionFactor(ENCODER_VELOCITY_CONVERSION);
-        config.apply(enc);
+        config
+                .encoder
+                .positionConversionFactor(ENCODER_POSITION_CONVERSION_FACTOR)
+                .velocityConversionFactor(ENCODER_VELOCITY_CONVERSION_FACTOR);
 
-        motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        config
+                .closedLoop
+                .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder)
+                .pidf(0.0001, 0.0, 0.0, 0.0);
 
-        encoder = motor.getEncoder();
+        config
+                .signals
+                .primaryEncoderVelocityAlwaysOn(true)
+                .primaryEncoderVelocityPeriodMs(20)
+                .appliedOutputPeriodMs(20)
+                .busVoltagePeriodMs(20)
+                .outputCurrentPeriodMs(20);
 
+        motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
     }
 
     @Override
     public void updateInputs(ShooterIOInputs inputs) {
         inputs.temperature = motor.getMotorTemperature();
         inputs.appliedVolts = motor.getBusVoltage() * motor.getAppliedOutput();
         inputs.currentAmps = motor.getOutputCurrent();
-        inputs.velocity = encoder.getVelocity();
+        inputs.velocityRPM = encoder.getVelocity();
+        inputs.targetVelocityRPM = targetVelocityRPM;
+
+        inputs.atTargetVelocity =
+                targetVelocityRPM > 0 &&
+                        Math.abs(targetVelocityRPM - inputs.velocityRPM) < TOLERANCE;
     }
 
+    @Override
     public void setPercent(double percent) {
         motor.set(percent);
     }
 
+    @Override
+    public void setVelocity(double velocityRPM) {
+        this.targetVelocityRPM = velocityRPM;
+        controller.setReference(velocityRPM, ControlType.kVelocity);
+    }
+
+    @Override
     public void setVoltage(double volts) {
         motor.setVoltage(volts);
     }
 
+    @Override
     public void stop() {
+        targetVelocityRPM = 0.0;
         motor.set(0);
     }
-}
+}