auto aim should work

Vortx3735 · Apr 3, 2024 · f67b811 · f67b811
1 parent b59338f
commit f67b811
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/commands/AutoAim.java b/src/main/java/frc/robot/commands/AutoAim.java
@@ -116,6 +116,7 @@ public static double[] stupid(double xDist, double yDist) {
         if(shootingvelo > 5700) {
             shootingvelo = 5700;
         }
+        //divide by 2 because theres 2 shooters that act on the note also this makes our trajectory work in desmos :)
         double rpm = (30*shootingvelo)/(Math.PI*(ShooterConstants.shooterRadius))/2;
 
         double t4 = (Math.pow(a_x, 2) + Math.pow(a_y, 2))/4;
@@ -126,7 +127,7 @@ public static double[] stupid(double xDist, double yDist) {
 
 
         List<Double> roots = RootFinder.rootFinder(Arrays.asList(t4, t3, t2, t1, t0), 0.01);
-        for(double root: roots) {
+        for(double root : roots) {
             root = Math.abs(root);
         }
         double t = -1;

diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
@@ -143,7 +143,7 @@ public double getArmPos() {
   }
 
   public double getRadiansTravelled() {
-    return armEncoder.getDistance()/(Constants.ArmConstants.encoderPitchDiameter/2);
+    return (position - ArmConstants.groundArmPos) * 2 * Math.PI;
   }
 
   public BooleanSupplier getArmDown() {