cleanup code

MiamiBeachBots · Jan 9, 2024 · b09ec01 · b09ec01
1 parent e94fb97
commit b09ec01
Show file tree

Hide file tree

Showing 3 changed files with 111 additions and 24 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -20,10 +20,10 @@ public static final class CANConstants {
     public static final int MOTORFRONTLEFTID = 13;
     public static final int MOTORBACKLEFTID = 14;
     /// Arm Motors
-    public static final int MOTORARMID = 21;
+    public static final int MOTORARMMAINID = 21;
+    public static final int MOTORARMSECONDARYID = 22;
     /// Shooter Motors
-    public static final int MOTORSHOOTERLEFTID = 31;
-    public static final int MOTORSHOOTERRIGHTID = 32;
+    public static final int MOTORSHOOTERID = 31;
   }
 
   public static final double MAX_SPEED = 0.8;

diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
@@ -12,20 +12,29 @@
 import frc.robot.Constants.CANConstants;
 
 public class ArmSubsystem extends SubsystemBase {
-  private final CANSparkMax m_armMotor;
-  private final SparkPIDController m_armPIDController;
-  private RelativeEncoder m_encoder;
+  private final CANSparkMax m_armMotorMain, m_armMotorSecondary;
+  private final SparkPIDController m_armMainPIDController;
+  private RelativeEncoder m_MainEncoder;
   public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, kLoweredArmPosition;
 
   /** Creates a new ArmSubsystem. */
   public ArmSubsystem() {
-    // create the arm motor
-    m_armMotor = new CANSparkMax(CANConstants.MOTORARMID, CANSparkMax.MotorType.kBrushless);
-    m_armMotor.setIdleMode(CANSparkMax.IdleMode.kBrake);
+    // create the arm motors
+    m_armMotorMain = new CANSparkMax(CANConstants.MOTORARMMAINID, CANSparkMax.MotorType.kBrushless);
+    m_armMotorSecondary =
+        new CANSparkMax(CANConstants.MOTORARMMAINID, CANSparkMax.MotorType.kBrushless);
+
+    // set the idle mode to brake
+    m_armMotorMain.setIdleMode(CANSparkMax.IdleMode.kBrake);
+    m_armMotorSecondary.setIdleMode(CANSparkMax.IdleMode.kBrake);
+
+    // set the secondary motor to follow the main motor
+    m_armMotorSecondary.follow(m_armMotorMain);
+
     // connect to built in PID controller
-    m_armPIDController = m_armMotor.getPIDController();
+    m_armMainPIDController = m_armMotorMain.getPIDController();
     // allow us to read the encoder
-    m_encoder = m_armMotor.getEncoder();
+    m_MainEncoder = m_armMotorMain.getEncoder();
     // PID coefficients
     kP = 0.1;
     kI = 1e-4;
@@ -37,41 +46,46 @@ public ArmSubsystem() {
     kLoweredArmPosition = 0;
 
     // set PID coefficients
-    m_armPIDController.setP(kP);
-    m_armPIDController.setI(kI);
-    m_armPIDController.setD(kD);
-    m_armPIDController.setIZone(kIz);
-    m_armPIDController.setFF(kFF);
-    m_armPIDController.setOutputRange(kMinOutput, kMaxOutput);
+    m_armMainPIDController.setP(kP);
+    m_armMainPIDController.setI(kI);
+    m_armMainPIDController.setD(kD);
+    m_armMainPIDController.setIZone(kIz);
+    m_armMainPIDController.setFF(kFF);
+    m_armMainPIDController.setOutputRange(kMinOutput, kMaxOutput);
   }
 
   /*
    * Move arm relative to current position
    */
   public void MoveArmRelative(double rotations) {
-    rotations = rotations + m_encoder.getPosition();
+    rotations = rotations + m_MainEncoder.getPosition();
     // update the PID controller with current encoder position
-    m_armPIDController.setReference(rotations, CANSparkBase.ControlType.kPosition);
+    MoveArmToPosition(rotations);
   }
 
   public void lowerArm() {
-    // update the PID controller with current encoder position
-    m_armPIDController.setReference(kLoweredArmPosition, CANSparkBase.ControlType.kPosition);
+    // move to set lowered arm position
+    MoveArmToPosition(kLoweredArmPosition);
   }
 
   /*
    * Move arm to global position
    */
   public void MoveArmToPosition(double rotations) {
     // update the PID controller with current encoder position
-    m_armPIDController.setReference(rotations, CANSparkBase.ControlType.kPosition);
+    m_armMainPIDController.setReference(rotations, CANSparkBase.ControlType.kPosition);
   }
 
   /*
    * attempt to hold arm at current location
    */
   public void stop() {
-    m_armPIDController.setReference(m_encoder.getPosition(), CANSparkBase.ControlType.kPosition);
+    // update the PID controller with current encoder position
+    MoveArmToPosition(m_MainEncoder.getPosition());
+  }
+
+  public void zero() {
+    m_MainEncoder.setPosition(0);
   }
 
   @Override

diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -4,12 +4,85 @@
 
 package frc.robot.subsystems;
 
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkPIDController;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.CANConstants;
 
 public class ShooterSubsystem extends SubsystemBase {
+  private final CANSparkMax m_ShooterMotorMain;
+  private final SparkPIDController m_ShooterMainPIDController;
+  private RelativeEncoder m_ShooterMainEncoder;
+  public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, maxRPM;
+
   /** Creates a new ShooterSubsystem. */
   public ShooterSubsystem() {
-    // TODO: Implement ShooterSubsystem
+    // create the shooter motors
+    m_ShooterMotorMain =
+        new CANSparkMax(CANConstants.MOTORSHOOTERID, CANSparkMax.MotorType.kBrushless);
+    // set the idle mode to coast
+    m_ShooterMotorMain.setIdleMode(CANSparkMax.IdleMode.kCoast);
+
+    // connect to built in PID controller
+    m_ShooterMainPIDController = m_ShooterMotorMain.getPIDController();
+
+    // allow us to read the encoder
+    m_ShooterMainEncoder = m_ShooterMotorMain.getEncoder();
+    // PID coefficients
+    kP = 6e-5;
+    kI = 0;
+    kD = 0;
+    kIz = 0;
+    kFF = 0.000015;
+    kMaxOutput = 1;
+    kMinOutput = -1;
+    maxRPM = 5700;
+
+    // set PID coefficients
+    m_ShooterMainPIDController.setP(kP);
+    m_ShooterMainPIDController.setI(kI);
+    m_ShooterMainPIDController.setD(kD);
+    m_ShooterMainPIDController.setIZone(kIz);
+    m_ShooterMainPIDController.setFF(kFF);
+    m_ShooterMainPIDController.setOutputRange(kMinOutput, kMaxOutput);
+  }
+
+  /*
+   * Spin shooter at a given RPM
+   */
+  public void SpinShooter(double RPM) {
+    m_ShooterMainPIDController.setReference(RPM, CANSparkBase.ControlType.kVelocity);
+  }
+
+  /*
+   * Stop the shooter
+   */
+  public void StopShooter() {
+    SpinShooter(0);
+  }
+
+  /*
+   * Spin Shooter at max RPM
+   */
+  public void SpinShooterFull() {
+    SpinShooter(maxRPM);
+  }
+
+  /*
+   * Check if shooter is at a given RPM
+   */
+  public Boolean isAtRPMTolerance(double RPM) {
+    return (m_ShooterMainEncoder.getVelocity() > RPM - 100
+        && m_ShooterMainEncoder.getVelocity() < RPM + 100);
+  }
+
+  /*
+   * Check if shooter is at max RPM
+   */
+  public Boolean isAtMaxRPM() {
+    return isAtRPMTolerance(maxRPM);
   }
 
   @Override