shooter testing changes

grt192 · Feb 2, 2024 · 639a217 · 639a217
1 parent de020f6
commit 639a217
Show file tree

Hide file tree

Showing 6 changed files with 50 additions and 22 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -97,7 +97,7 @@ public static class ShooterConstants {
     public static final int SHOOTER_MOTOR_ONE_ID = 13;
     public static final int SHOOTER_MOTOR_TWO_ID = 14;
 
-    public static final int FEEDER_MOTOR_ID = 10;
+    public static final int FEEDER_MOTOR_ID = 15;
   }
 
   public static class ClimbConstants {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -17,6 +17,8 @@
 import frc.robot.commands.climb.ClimbRaiseCommand;
 import frc.robot.commands.elevator.ElevatorToAMPCommand;
 import frc.robot.commands.elevator.ElevatorToGroundCommand;
+import frc.robot.commands.shooter.pivot.ShooterPivotSetAngle;
+import frc.robot.commands.shooter.pivot.ShooterPivotVerticalCommand;
 import frc.robot.subsystems.climb.ClimbSubsystem;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.swerve.BaseSwerveSubsystem;
@@ -114,10 +116,10 @@ private void configureBindings() {
         shooterFeederSubsystem.setFeederMotorSpeed(0);
       }));
 
-      shooterPivotSubsystem.setDefaultCommand(new InstantCommand(() -> {
-          // shooterPivotSubsystem.setPivotMotorSpeed((.2 * mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis()));
-          // pivotSubsystem.printCurrentAngle();
-      }, shooterPivotSubsystem));
+      // shooterPivotSubsystem.setDefaultCommand(new InstantCommand(() -> {
+      //     shooterPivotSubsystem.setPivotMotorSpeed((.2 * mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis()));
+      //     // pivotSubsystem.printCurrentAngle();
+      // }, shooterPivotSubsystem));
 
       aButton.onTrue(new InstantCommand(() -> {
         shooterFlywheelSubsystem.setShooterMotorSpeed(shooterFlywheelSubsystem.SHOOTER_MOTOR_SPEED);
@@ -137,13 +139,8 @@ private void configureBindings() {
         }
       }, intakeRollerSubsystem));
 
-      rightBumper.onTrue(new ElevatorToAMPCommand(elevatorSubsystem));
-      leftBumper.onTrue(new ElevatorToGroundCommand(elevatorSubsystem));
-
-
-
-
-
+      rightBumper.onTrue(new ShooterPivotSetAngle(shooterPivotSubsystem, Math.toRadians(40)));
+      leftBumper.onTrue(new ShooterPivotSetAngle(shooterPivotSubsystem, Math.toRadians(60)));
 
       if(baseSwerveSubsystem instanceof SwerveSubsystem){
         final SwerveSubsystem swerveSubsystem = (SwerveSubsystem) baseSwerveSubsystem;

diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java
@@ -22,4 +22,12 @@ public void initialize() {
     public boolean isFinished() {
         return (Math.abs(pivotSubsystem.getPosition() - angle) < pivotSubsystem.ERRORTOLERANCE);
     }
+
+    public void end(boolean interrupted) {
+        if(interrupted){
+            System.out.println("ANGLE INTERRUPTED");
+        } else {
+            System.out.println("ANGLE ARRIVED");
+        }
+    }
 }
diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java
@@ -13,11 +13,20 @@ public ShooterPivotVerticalCommand(ShooterPivotSubsystem pivotSubsystem){
     @Override
     public void initialize() {
         pivotSubsystem.setAutoAimBoolean(false);
-        pivotSubsystem.setAngle(90.0);
+        pivotSubsystem.setAngle(0.0);
     }
 
     @Override
     public boolean isFinished() {
-        return (Math.abs(pivotSubsystem.getPosition() - 90) < pivotSubsystem.ERRORTOLERANCE);
+        return (Math.abs(pivotSubsystem.getPosition()) < pivotSubsystem.ERRORTOLERANCE);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        if(interrupted){
+            System.out.println("VERTICAL INTERRUPTED");
+        } else {
+            System.out.println("VERTICAL ARRIVED");
+        }
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java
@@ -117,7 +117,7 @@ else if(currentTargetState.equals(ElevatorState.TRAP)){
     public void periodic(){
         //System.out.println(elevatorNetworkTablePositionEntry.getString("default"));
         if(timer.advanceIfElapsed(.2)){
-            System.out.println(Units.metersToInches(getExtensionMeters()));
+            // System.out.println(Units.metersToInches(getExtensionMeters()));
         }
 
         //System.out.println(this.getTargetState());

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import static frc.robot.Constants.ShooterConstants.*;
 
@@ -17,7 +18,7 @@ public class ShooterPivotSubsystem extends SubsystemBase {
     //final vars
     public final double PIVOT_SPEED = 0.1;
     final double GEARBOX_RATIO = 18.16; //ask cadders
-    public final int ERRORTOLERANCE = 5; //error tolerance for pid
+    public final double ERRORTOLERANCE = Math.toRadians(2); //error tolerance for pid
     final int LIMIT_SWITCH_ID = 3; //placeholder
     final double CONVERSION_FACTOR = Math.PI/(2.*4.57);
 
@@ -30,9 +31,10 @@ public class ShooterPivotSubsystem extends SubsystemBase {
     private final DigitalInput limitSwitch;
 
     //angle PID (CHANGE LATER)
-    private static final double ANGLE_P = 2.4;
-    private static final double ANGLE_I = 0;
-    private static final double ANGLE_D = 0;
+    private static final double ANGLE_P = 0.5;
+    private static final double ANGLE_I = 0.001;
+    private static final double ANGLE_D = 15;
+    private static final double ANGLE_FF = -.5;
 
     //field
     private boolean alliance; //true equals red alliance 
@@ -49,8 +51,12 @@ public class ShooterPivotSubsystem extends SubsystemBase {
     double BLUE_X = Units.inchesToMeters(-1.5+9.05); //9.05 is half of 18.1 which is length of overhang of speaker-- we want halfway point
     double BLUE_Y = Units.inchesToMeters(218.42);
 
+    private final Timer timer = new Timer();
+
     public ShooterPivotSubsystem(boolean alliance){
 
+        timer.start();
+
         //motors
         pivotMotor = new CANSparkMax(PIVOT_MOTOR_ID, MotorType.kBrushless); 
         pivotMotor.setInverted(true);
@@ -59,13 +65,16 @@ public ShooterPivotSubsystem(boolean alliance){
         rotationEncoder = pivotMotor.getEncoder();
         rotationEncoder.setPosition(0); 
         rotationPIDController = pivotMotor.getPIDController();
+        rotationPIDController.setOutputRange(-.4, 0.07);
         limitSwitch = new DigitalInput(LIMIT_SWITCH_ID);
 
 
         //setting PID vars
         rotationPIDController.setP(ANGLE_P);
         rotationPIDController.setI(ANGLE_I);
         rotationPIDController.setD(ANGLE_D);
+        rotationPIDController.setFF(0);
+        System.out.println(rotationPIDController.getFF());
 
         //encoder stuff
         rotationEncoder.setPositionConversionFactor(CONVERSION_FACTOR);
@@ -87,7 +96,8 @@ public void setPivotMotorSpeed(double speed){
 
     public void setAngle(double angle){ //check if it works 
         rotationPIDController.setReference(angle, CANSparkMax.ControlType.kPosition);
-        //System.out.println("setting angle to: " + angle);
+        System.out.println("setting angle to: " + angle);
+
     }
 
     public void setFieldPosition(Pose2d field){
@@ -115,7 +125,9 @@ public double getAutoAimAngle(){
     }
 
     public void printCurrentAngle(){
-        System.out.println("radians: " + rotationEncoder.getPosition() + "degrees: " + rotationEncoder.getPosition() * 57.29);
+        System.out.println("radians: " + rotationEncoder.getPosition() + "  degrees: " + rotationEncoder.getPosition() * 57.29);
+        // System.out.println(pivotMotor.get());
+        System.out.println(rotationPIDController.getFF());
     }
 
     public double getPosition(){
@@ -143,7 +155,9 @@ public void periodic(){
             setAngle(getAutoAimAngle());
         }
 
-        // printCurrentAngle();
+        if(timer.advanceIfElapsed(.2)){ 
+            printCurrentAngle();
+        }
 
         // System.out.println("current pos" + rotationEncoder.getPosition());