FIXED TURRET LOGIC IT WAS INSANELY STUPID BEFORE

StuyPulse · Dec 13, 2023 · fde7999 · fde7999
1 parent 166d48e
commit fde7999
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 10 deletions.
diff --git a/src/main/java/com/stuypulse/robot/commands/TurretPoint.java b/src/main/java/com/stuypulse/robot/commands/TurretPoint.java
@@ -12,7 +12,7 @@ public class TurretPoint extends CommandBase {
     private final Turret turret;
     private final Odometry odometry;
 
-    // private Translation2d target;
+    private Translation2d target;
 
     public TurretPoint(Translation2d target) {
         //this.target = target;

diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java
@@ -44,15 +44,15 @@ public Turret() {
 
     public void setTargetAngle(double angle, double maxTarget, double minTarget) {
         // closest to current angle as possible
-        double realTargetAngle = getTurretAngle() + (angle);
-
-        if (realTargetAngle > maxTarget) {
-            targetAngle.set(realTargetAngle - (360 * (Math.ceil(realTargetAngle / 360))));
-        } else if (realTargetAngle < minTarget) {
-            targetAngle.set(realTargetAngle + (360 * (Math.ceil(realTargetAngle / 360))));
-        } else {
-            // targetAngle.set(angle % 360); // originally 360
-            targetAngle.set(realTargetAngle);
+
+        if (angle > maxTarget) {
+            targetAngle.set(angle % maxTarget);
+        }
+        if (angle < minTarget) {
+            targetAngle.set(angle % minTarget);
+        } 
+        else {
+            targetAngle.set(angle % 360);
         }
     }