fixed distance functions

grt192 · Feb 2, 2024 · 35f7043 · 35f7043
1 parent 17abb8b
commit 35f7043
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 30 deletions.
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,10 +21,7 @@ public void end(){
     }
 
     public boolean isFinished(){
-        if(Math.abs(shooterPivotSubsystem.getPosition()) - shooterPivotSubsystem.getCurrentAngle() < shooterPivotSubsystem.ERRORTOLERANCE){
-            return true;
-        }
+        return(Math.abs(shooterPivotSubsystem.getPosition() - shooterPivotSubsystem.getAutoAimAngle()) < shooterPivotSubsystem.ERRORTOLERANCE);
 
-        return false;
     }
 }
diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java
@@ -20,10 +20,6 @@ public void initialize() {
 
     @Override
     public boolean isFinished() {
-        if(Math.abs(pivotSubsystem.getPosition()) - angle < pivotSubsystem.ERRORTOLERANCE){
-            return true;
-        }
-
-        return false;
+        return (Math.abs(pivotSubsystem.getPosition() - angle) < pivotSubsystem.ERRORTOLERANCE);
     }
 }
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,10 +18,6 @@ public void initialize() {
 
     @Override
     public boolean isFinished() {
-        if(Math.abs(pivotSubsystem.getPosition()) - 90 < pivotSubsystem.ERRORTOLERANCE){
-            return true;
-        }
-
-        return false;
+        return (Math.abs(pivotSubsystem.getPosition() - 90) < pivotSubsystem.ERRORTOLERANCE);
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java
@@ -38,6 +38,7 @@ public class ShooterPivotSubsystem extends SubsystemBase {
     private boolean alliance; //true equals red alliance 
     private boolean autoAim;
     private double currentEncoderAngle;
+    private double currentDistance;
     private Pose2d currentField = new Pose2d();
 
     //center of red speaker: (652.73 218.42)
@@ -92,34 +93,31 @@ public void setAngle(double angle){ //check if it works
     public void setFieldPosition(Pose2d field){
         //System.out.println("setting field position");
         currentField = field;
-    }
-
-    public double getAutoAimAngle(double distance){
-        double speakerHeight = Units.inchesToMeters(80.51);
-        //System.out.println("Angle of shooter" + Math.atan(speakerHeight/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
             double xLength = Math.pow(currentField.getX()-RED_X, 2);
             double yLength = Math.pow(currentField.getY()-RED_Y, 2);
             //System.out.println("alliance red:" + alliance);
-            return Math.sqrt(xLength + yLength);
+            currentDistance = Math.sqrt(xLength + yLength);
 
         } else {
             double xLength = Math.pow(currentField.getX()-BLUE_X, 2);
             double yLength = Math.pow(currentField.getY()-BLUE_Y, 2);
 
-            return Math.sqrt(xLength + yLength);
+            currentDistance = Math.sqrt(xLength + yLength);
         } 
     }
 
+    public double getAutoAimAngle(){
+        double speakerHeight = Units.inchesToMeters(80.51);
+        //System.out.println("Angle of shooter" + Math.atan(speakerHeight/distance));
+        return Math.atan(speakerHeight/currentDistance);
+    }
+
+    public void printCurrentAngle(){
+        System.out.println("radians: " + rotationEncoder.getPosition() + "degrees: " + rotationEncoder.getPosition() * 57.29);
+    }
+
     public double getPosition(){
         //System.out.println("rotation encoder position: " + rotationEncoder.getPosition());
         return rotationEncoder.getPosition();
@@ -142,7 +140,7 @@ public void periodic(){
         // }
 
         if(autoAim){
-            setAngle(getAutoAimAngle(getDistance()));
+            setAngle(getAutoAimAngle());
         }
 
         // printCurrentAngle();