testing shooter code

grt192 · Jan 31, 2024 · 1916a9d · 1916a9d
1 parent 64ce150
commit 1916a9d
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 6 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -119,7 +119,8 @@ private void configureBindings() {
         }));
 
         pivotSubsystem.setDefaultCommand(new InstantCommand(() -> {
-            pivotSubsystem.setPivotMotorSpeed(.3 * (mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis()));
+            pivotSubsystem.setPivotMotorSpeed((.2 * mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis()));
+            // pivotSubsystem.printCurrentAngle();
         }, pivotSubsystem));
 
         if(baseSwerveSubsystem instanceof SwerveSubsystem){

diff --git a/src/main/java/frc/robot/subsystems/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterPivotSubsystem.java
@@ -17,6 +17,7 @@ public class ShooterPivotSubsystem extends SubsystemBase {
     final double GEARBOX_RATIO = 18.16; //ask cadders
     public final int ERRORTOLERANCE = 5; //error tolerance for pid
     final int LIMIT_SWITCH_ID = 1; //placeholder
+    final double CONVERSION_FACTOR = Math.PI/(2*4.57);
 
     //motors
     private final CANSparkMax pivotMotor;
@@ -49,6 +50,7 @@ public ShooterPivotSubsystem(boolean alliance){
 
         //motors
         pivotMotor = new CANSparkMax(12, MotorType.kBrushless); 
+        pivotMotor.setInverted(true);
 
         //devices
         rotationEncoder = pivotMotor.getEncoder();
@@ -63,7 +65,8 @@ public ShooterPivotSubsystem(boolean alliance){
         rotationPIDController.setD(ANGLE_D);
 
         //encoder stuff
-        rotationEncoder.setPositionConversionFactor(GEARBOX_RATIO);
+        rotationEncoder.setPositionConversionFactor(CONVERSION_FACTOR);
+        //rotationEncoder.setInverted(true);
         rotationPIDController.setSmartMotionAllowedClosedLoopError(ERRORTOLERANCE, 0); //what does 0 do (slotID is from 0-3)
 
         //field
@@ -94,6 +97,10 @@ public double getAutoAimAngle(double distance){
         return Math.atan(speakerHeight/distance);
     }
 
+    public void printCurrentAngle(){
+        System.out.println("radians: " + rotationEncoder.getPosition() + "degrees: " + rotationEncoder.getPosition() * 57.29);
+    }
+
     public double getDistance(){
 
         if(alliance){ //true = red
@@ -126,15 +133,17 @@ public void setAutoAimBoolean(boolean red){
     public void periodic(){
         //resets relative encoder every time robot starts again
         //(check if encoder prints zero when run)
-        if(limitSwitch != null && limitSwitch.get()){ //false = limit switch is pressed
-            rotationEncoder.setPosition(0); 
-            // System.out.println(rotationEncoder.getPosition()); //should print 0
-        }
+        // if(limitSwitch != null && limitSwitch.get()){ //false = limit switch is pressed
+        //     rotationEncoder.setPosition(0); 
+        //     // System.out.println(rotationEncoder.getPosition()); //should print 0
+        // }
 
         if(autoAim){
             setAngle(getAutoAimAngle(getDistance()));
         }
 
+        printCurrentAngle();
+
         // System.out.println("current pos" + rotationEncoder.getPosition());
 
         // if(currentState == ShooterState.FIRING && (shooterSensor.getRed() < TOLERANCE)){  //when there is no note