shooter formatting

grt192 · Feb 25, 2024 · 1febb20 · 1febb20
1 parent d6fecf1
commit 1febb20
Show file tree

Hide file tree

Showing 6 changed files with 28 additions and 36 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -121,16 +121,29 @@ public static class IntakeConstants {
 
     /** Constants for Shooter Subsystem. */
     public static class ShooterConstants {
-        public static final int LIMIT_SWITCH_ID = 1;
+        //pivot constants
+        public static final int LIMIT_SWITCH_ID = 4;
         public static final int PIVOT_MOTOR_ID = 12;
+        public static final double CONVERSION_FACTOR = Units.degreesToRadians(44) / 6.33;
+        public static final double PID_ERROR_TOLERANCE = Math.toRadians(2); //error tolerance for pid
 
+        //flywheel constants
         public static final int SHOOTER_MOTOR_TOP_ID = 13;
         public static final int SHOOTER_MOTOR_BOTTOM_ID = 14;
+        public static final double SHOOTER_MOTOR_SPEED = 1;
+
+        //field constants
+        //center of red speaker: (652.73 218.42)
+        public static final double RED_X = Units.inchesToMeters(652.73 + 9.05);
+        public static final double RED_Y = Units.inchesToMeters(218.42);
+
+        //center of blue speaker: (-1.50 218.42)
+        public static final double BLUE_X = Units.inchesToMeters(-1.5 + 9.05);
+        public static final double BLUE_Y = Units.inchesToMeters(218.42);
+
+
 
-        public static final int FEEDER_MOTOR_ID = 15;
-        // public static final int FEEDER_MOTOR_2_ID = 43; //CHANGE THIS
 
-        public static final double FEED_ANGLE = Units.degreesToRadians(70);
     }
 
     /** Constants for Climb Subsystem. */

diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotAimCommand.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotAimCommand.java
@@ -21,7 +21,7 @@ public void end(){
     }
 
     public boolean isFinished(){
-        return(Math.abs(shooterPivotSubsystem.getPosition() - shooterPivotSubsystem.getAutoAimAngle()) < shooterPivotSubsystem.ERRORTOLERANCE);
+        return(Math.abs(shooterPivotSubsystem.getPosition() - shooterPivotSubsystem.getAutoAimAngle()) < shooterPivotSubsystem.PID_ERROR_TOLERANCE);
 
     }
 }
diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngleCommand.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngleCommand.java
@@ -22,7 +22,7 @@ public void initialize() {
     @Override
     public boolean isFinished() {
         System.out.println("NOT FINISHED" + Math.abs(pivotSubsystem.getPosition() - angle));
-        return (Math.abs(pivotSubsystem.getPosition() - angle) < pivotSubsystem.ERRORTOLERANCE);
+        return (Math.abs(pivotSubsystem.getPosition() - angle) < pivotSubsystem.PID_ERROR_TOLERANCE);
     }
 
     public void end(boolean interrupted) {

diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java
@@ -18,7 +18,7 @@ public void initialize() {
 
     @Override
     public boolean isFinished() {
-        return (Math.abs(pivotSubsystem.getPosition()) < pivotSubsystem.ERRORTOLERANCE);
+        return (Math.abs(pivotSubsystem.getPosition()) < pivotSubsystem.PID_ERROR_TOLERANCE);
     }
 
     @Override

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java
@@ -9,16 +9,11 @@
 
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
-import com.revrobotics.CANSparkBase.IdleMode;
-import com.revrobotics.CANSparkLowLevel.MotorType;
-import com.revrobotics.CANSparkMax;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 /** Inits motors and state enums for shooter subsystem. */
 public class ShooterFlywheelSubsystem extends SubsystemBase {
 
-    public final double SHOOTER_MOTOR_SPEED = 1;
-
     //motors
     private final TalonFX shooterMotorTop;
     private final TalonFX shooterMotorBottom;

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java
@@ -17,12 +17,6 @@
 /** Controls motors and functions for the pivot part of shooter mech. */
 public class ShooterPivotSubsystem extends SubsystemBase {
 
-    //final vars
-    final double GEARBOX_RATIO = 18.16; //ask cadders
-    public final double ERRORTOLERANCE = Math.toRadians(2); //error tolerance for pid
-    final int LIMIT_SWITCH_ID = 4; //placeholder
-    final double CONVERSION_FACTOR = Units.degreesToRadians(44) / 6.33;
-
     //motors
     private final CANSparkMax pivotMotor;
 
@@ -43,21 +37,12 @@ public class ShooterPivotSubsystem extends SubsystemBase {
     private double currentDistance;
     private Pose2dSupplier poseSupplier; //new Pose2d();
 
-    //center of red speaker: (652.73 218.42)
-    double RED_X = Units.inchesToMeters(652.73 + 9.05);
-    double RED_Y = Units.inchesToMeters(218.42);
-
-    //center of blue speaker: (-1.50 218.42)
-    double BLUE_X = Units.inchesToMeters(-1.5+9.05);
-    double BLUE_Y = Units.inchesToMeters(218.42);
-
     private final Timer timer = new Timer();
 
     /** Inits motors and pose field. Also inits PID stuff. */
     public ShooterPivotSubsystem(boolean alliance, Pose2dSupplier poseSupplier){
 
         timer.start();
-
         this.poseSupplier = poseSupplier;
 
         //motors
@@ -70,7 +55,7 @@ public ShooterPivotSubsystem(boolean alliance, Pose2dSupplier poseSupplier){
         rotationEncoder.setPosition(0); 
         rotationPIDController = pivotMotor.getPIDController();
         rotationPIDController.setOutputRange(-.4, 0.07);
-        limitSwitch = new DigitalInput(LIMIT_SWITCH_ID);
+        limitSwitch = new DigitalInput(ShooterConstants.LIMIT_SWITCH_ID);
 
 
         //setting PID vars
@@ -81,10 +66,10 @@ public ShooterPivotSubsystem(boolean alliance, Pose2dSupplier poseSupplier){
         System.out.println(rotationPIDController.getFF());
 
         //encoder stuff
-        rotationEncoder.setPositionConversionFactor(CONVERSION_FACTOR);
-        rotationEncoder.setVelocityConversionFactor(CONVERSION_FACTOR * 60);
+        rotationEncoder.setPositionConversionFactor(ShooterConstants.CONVERSION_FACTOR);
+        rotationEncoder.setVelocityConversionFactor(ShooterConstants.CONVERSION_FACTOR * 60);
         rotationEncoder.setPosition(Units.degreesToRadians(18));
-        rotationPIDController.setSmartMotionAllowedClosedLoopError(ERRORTOLERANCE, 0); 
+        rotationPIDController.setSmartMotionAllowedClosedLoopError(ShooterConstants.PID_ERROR_TOLERANCE, 0); 
 
         //pivot soft limits
         pivotMotor.setSoftLimit(SoftLimitDirection.kForward, (float) Units.degreesToRadians(62));
@@ -117,14 +102,13 @@ public double getAutoAimAngle() {
         //System.out.println("Angle of shooter" + Math.atan(speakerHeight/distance));
 
         if (alliance) {  //true = red
-            double xLength = Math.pow(currentField.getX() - RED_X, 2);
-            double yLength = Math.pow(currentField.getY() - RED_Y, 2);
-            //System.out.println("alliance red:" + alliance);
+            double xLength = Math.pow(currentField.getX() - ShooterConstants.RED_X, 2);
+            double yLength = Math.pow(currentField.getY() - ShooterConstants.RED_Y, 2);
             currentDistance = Math.sqrt(xLength + yLength);
 
         } else {
-            double xLength = Math.pow(currentField.getX() - BLUE_X, 2);
-            double yLength = Math.pow(currentField.getY() - BLUE_Y, 2);
+            double xLength = Math.pow(currentField.getX() - ShooterConstants.BLUE_X, 2);
+            double yLength = Math.pow(currentField.getY() - ShooterConstants.BLUE_Y, 2);
 
             currentDistance = Math.sqrt(xLength + yLength);
         }