fix: excise ballerinas from robot code

src/main/java/frc4388/robot/commands/GotoLastApril.java

@@ -77,9 +77,25 @@ public class GotoLastApril extends Command {
     @Override
     public void execute() {
         xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
-        yerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.YAxis);
+        yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis);
+        // xerr = targetpos.getX() - vision.getPose2d().getX();
+        // yerr = targetpos.getX() - vision.getPose2d().getY();
 
-        roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
+        // roterr = TimesNegativeOne.invert(targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(), TimesNegativeOne.isRed);
+
+        roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees()));
+
+        boolean invert = Math.abs(roterr) > 180;
+
+        if(roterr > 180){
+            roterr -= 360;
+        }else if(roterr < -180){
+            roterr += 360;
+        }
+
+        SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
+        SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
+        SmartDashboard.putNumber("Rotational PID error", roterr);
 
         // SmartDashboard.putNumber("PID X Error", xerr);
         // SmartDashboard.putNumber("PID Y Error", yerr);

src/main/java/frc4388/robot/commands/LidarAlign.java

@@ -21,6 +21,7 @@ public class LidarAlign extends Command {
   private boolean foundReef;
   private boolean headedRight;
   private double speed;
+  private int bounces;
   // private final boolean constructedHeadedRight;
 
   /** Creates a new LidarAlign. */
@@ -40,6 +41,7 @@ public class LidarAlign extends Command {
     this.speed = 0.4; // TODO: find good speed for this
     this.foundReef = false;
     this.headedRight = !(GotoLastApril.tagRelativeXError < 0);
+    this.bounces = 0;
   }
 
 
@@ -55,6 +57,8 @@ public class LidarAlign extends Command {
     if (currentFinderTick > 10) { //arbutrary threshhold for now.
       headedRight = !headedRight;
       currentFinderTick *= -1;
+      bounces++;
+      if (bounces == 2) return;
     }
     double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
     double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef
@@ -78,7 +82,10 @@ public class LidarAlign extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (foundReef && lidar.withinDistance()) { // spot on
+    if (lidar.getDistance() < 4) {
+      swerveDrive.stopModules();
+      return true;
+    } else if (foundReef && lidar.withinDistance()) { // spot on
       swerveDrive.stopModules();
       return true;
     } else if (foundReef && !lidar.withinDistance()) { // over shot
@@ -87,6 +94,9 @@ public class LidarAlign extends Command {
       currentFinderTick = 0;
       foundReef = false;
       return false;
+    } else if (bounces == 2) {
+      swerveDrive.stopModules();
+      return true;
     } else {
       return false;
     }

src/main/java/frc4388/robot/commands/MoveForTimeCommand.java

@@ -5,6 +5,7 @@ import java.time.Instant;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.TimesNegativeOne;
 
 // Command to repeat a joystick movement for a specific time.
 public class MoveForTimeCommand extends Command {