From 3c209063f68914a93ad2b92a53220b09211589db Mon Sep 17 00:00:00 2001
From: Cherriae <zeruiz@nycstudents.net>
Date: Sat, 20 Jan 2024 15:35:16 -0500
Subject: [PATCH] added feedforward + pid to shooter

---
 src/main/java/frc/robot/Constants.java        |  8 +++++
 .../robot/subsystems/ShooterSubsystem.java    | 29 ++++++++++++++++---
 2 files changed, 33 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index c03fa0e..3c7a4de 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -44,6 +44,8 @@ public static class Speeds {
 
     public static final double SWERVE_DRIVE_MAX_SPEED = 8.78; // TODO: Get this value
     public static final double SWERVE_DRIVE_MAX_ANGULAR_SPEED = Math.PI * 2; // Todo: Get this value
+
+    public static final double SHOOTER_MAX_SPEED = 1; 
   }
 
   public static class Physical {
@@ -55,6 +57,10 @@ public static class Physical {
     public static final double SWERVE_DRIVE_WHEEL_RADIUS = 0.1;
     public static final double SWERVE_DRIVE_WHEEL_CIRCUMFERENCE = 2 * Math.PI * SWERVE_DRIVE_WHEEL_RADIUS;
 
+    public static final double SHOOTER_GEAR_RATIO = 1.45;
+    public static final double SHOOTER_FLYWHEEL_RADIUS = 1; // TODO: FIND RADIUS
+    public static final double SHOOTER_FLYWHEEL_CIRCUMFERENCE = 2 * Math.PI * SHOOTER_FLYWHEEL_RADIUS;
+
 
     public static final double TALON_TICKS_PER_REVOLUTION = 2048;
 
@@ -64,6 +70,8 @@ public static class Physical {
       new Translation2d(-0.292, -0.292),
       new Translation2d(-0.292, 0.292)
     );
+
+    public static final double SHOOTER_PID_KP = 0;
   }
 
   public static class Offsets {
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
index b01d2dc..6b6be7c 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -3,10 +3,12 @@
 
 package frc.robot.subsystems;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
 /**
@@ -15,11 +17,17 @@
  * @author Cherine Soewingjo
  */
 public class ShooterSubsystem extends SubsystemBase {
-  private CANSparkMax _leftMotor = new CANSparkMax(Constants.CAN.SHOOTER_LEFT, MotorType.kBrushless);
-  private CANSparkMax _rightMotor = new CANSparkMax(Constants.CAN.SHOOTER_RIGHT, MotorType.kBrushless);
+  private final CANSparkMax _leftMotor = new CANSparkMax(Constants.CAN.SHOOTER_LEFT, MotorType.kBrushless);
+  private final CANSparkMax _rightMotor = new CANSparkMax(Constants.CAN.SHOOTER_RIGHT, MotorType.kBrushless);
+
+  private final RelativeEncoder _leftEncoder = _leftMotor.getEncoder();
+
+  private final PIDController _shooterController = new PIDController(Constants.Physical.SHOOTER_PID_KP, 0, 0); 
+
 
   /** Creates a new ShooterSubsystem. */
   public ShooterSubsystem() {
+    _rightMotor.follow(_leftMotor,true);
   }
 
 
@@ -30,11 +38,24 @@ public void periodic() {
 
   public void spinMotor(){
     _leftMotor.set(-1.0);
-    _rightMotor.set(1.0);
   }
 
   public void stopMotors(){
     _leftMotor.set(0);
-    _rightMotor.set(0);
+  }
+
+  /** Get the velocity of the back wheel (left side) in m/s. */
+  public double getShooterVelocity(){
+    double neo_rps = _leftEncoder.getVelocity() / 60;
+
+    return(neo_rps / Constants.Physical.SHOOTER_GEAR_RATIO ) * Constants.Physical.SHOOTER_FLYWHEEL_CIRCUMFERENCE;
+  }
+
+  /** Set the velocity of the back wheels in m/s. */
+  public void setVelocity(double velocity){
+    double flywheel_output = (velocity / Constants.Speeds.SHOOTER_MAX_SPEED); // FEEDFORWARD (main output)
+    double flywheel_pid = _shooterController.calculate(getShooterVelocity(), velocity); // PID for distrubances
+
+    _leftMotor.set(flywheel_output + flywheel_pid);
   }
 }