Merge pull request #36 from Team4388/Create-TimesNegativeOne

Add Create times negative one branch into Functional LEDS

src/main/java/frc4388/robot/Robot.java

@@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc4388.utility.CanDevice;
 import frc4388.utility.DeferredBlock;
+import frc4388.utility.DeferredBlockMulti;
 import frc4388.utility.RobotTime;
 import frc4388.utility.Status;
 import frc4388.utility.Subsystem;
@@ -113,6 +114,7 @@ public class Robot extends TimedRobot {
   @Override
   public void disabledExit() {
     DeferredBlock.execute();
+    DeferredBlockMulti.execute();
   }
 
   /**

src/main/java/frc4388/robot/RobotContainer.java

@@ -65,8 +65,10 @@ import frc4388.robot.subsystems.SwerveDrive;
 
 // Utilites
 import frc4388.utility.DeferredBlock;
+import frc4388.utility.DeferredBlockMulti;
 import frc4388.utility.ReefPositionHelper;
 import frc4388.utility.Subsystem;
+import frc4388.utility.TimesNegativeOne;
 import frc4388.utility.ReefPositionHelper.Side;
 import frc4388.utility.configurable.ConfigurableString;
 
@@ -324,7 +326,12 @@ public class RobotContainer {
 
         configureButtonBindings();        
         configureVirtualButtonBindings();
-        new DeferredBlock(() -> m_robotSwerveDrive.resetGyro());
+        new DeferredBlock(() -> { // Called on first robot enable
+            m_robotSwerveDrive.resetGyro();
+        });
+        new DeferredBlockMulti(() -> { // Called on every robot enable
+            TimesNegativeOne.update();
+        });
         DriverStation.silenceJoystickConnectionWarning(true);
         // CameraServer.startAutomaticCapture();
 
@@ -498,7 +505,6 @@ public class RobotContainer {
         new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180)
             .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown()));
 
-
         new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90)
             .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp()));
 

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

@@ -15,6 +15,7 @@ import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Vision;
 import frc4388.utility.Gains;
 import frc4388.utility.ReefPositionHelper;
+import frc4388.utility.TimesNegativeOne;
 import frc4388.utility.ReefPositionHelper.Side;
 import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
 import frc4388.utility.UtilityStructs.AutoRecordingFrame;
@@ -75,19 +76,26 @@ public class GotoLastApril extends Command {
 
     @Override
     public void execute() {
-        
-        if (alliance == Alliance.Red) {
-            xerr = -(targetpos.getX() - vision.getPose2d().getX());
-            yerr = (targetpos.getY() - vision.getPose2d().getY());
-        }
-        else{
-            
-            xerr = targetpos.getX() - vision.getPose2d().getX();
-            yerr = targetpos.getY() - vision.getPose2d().getY();
+        xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
+        yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis);
+        // xerr = targetpos.getX() - vision.getPose2d().getX();
+        // yerr = targetpos.getX() - vision.getPose2d().getY();
+
+        // 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;
         }
 
-            
-        roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
+        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 {

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -35,9 +35,8 @@ import frc4388.robot.commands.GotoLastApril;
 import frc4388.robot.commands.LidarAlign;
 import frc4388.utility.Status;
 import frc4388.utility.Subsystem;
+import frc4388.utility.TimesNegativeOne;
 import frc4388.utility.Status.ReportLevel;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.config.PIDConstants;
@@ -103,11 +102,11 @@ public class SwerveDrive extends Subsystem {
                     // This will flip the path being followed to the red side of the field.
                     // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
 
-                    var alliance = DriverStation.getAlliance();
-                    if (alliance.isPresent()) {
-                        return alliance.get() == DriverStation.Alliance.Red;
-                    }
-                    return false;
+                    // var alliance = DriverStation.getAlliance();
+                    // if (alliance.isPresent()) {
+                    //     return alliance.get() == DriverStation.Alliance.Red;
+                    // }
+                    return TimesNegativeOne.isRed;
                 },
                 this // Reference to this subsystem to set requirements
         );
@@ -140,23 +139,14 @@ public class SwerveDrive extends Subsystem {
         if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
             return; // don't bother doing swerve drive math and return early.
 
-        leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
-
-        Optional<Alliance> alliance = DriverStation.getAlliance();
-        
-        if(!alliance.isEmpty()){
-            if (alliance.get() == Alliance.Red) 
-                leftStick = new Translation2d(-leftStick.getX(), leftStick.getY());
-            else 
-                leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
-            // if (alliance.get() != Alliance.Red) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
-        }
-        if (SwerveDriveConstants.INVERT_ROTATION) rightStick.times(-1);
-
-
+            leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
         
         stopped = false;
         if (fieldRelative) {
+            
+            leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
+            rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis);    
+
             // ! drift correction
             if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) {
                 rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
@@ -218,7 +208,7 @@ public class SwerveDrive extends Subsystem {
         if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
             return; // don't bother doing swerve drive math and return early.
 
-        leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
+        leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
 
         swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
                 .withVelocityX(leftStick.getX() * speedAdjust)
@@ -282,7 +272,7 @@ public class SwerveDrive extends Subsystem {
         // if (leftStick.getNorm() < 0.05) //if no imput
         // return; // don't bother doing swerve drive math and return early.
 
-        leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
+        leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
 
         swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
                 .withVelocityX(leftStick.getX() * -speedAdjust)

src/main/java/frc4388/utility/Alliance.java

@@ -0,0 +1,5 @@
+package frc4388.utility;
+
+public class Alliance {
+
+}

src/main/java/frc4388/utility/DeferredBlockMulti.java

@@ -0,0 +1,18 @@
+package frc4388.utility;
+
+import java.util.ArrayList;
+
+public class DeferredBlockMulti {
+    private static ArrayList<Runnable> m_blocks = new ArrayList<>();
+
+    public DeferredBlockMulti(Runnable block) {
+        m_blocks.add(block);
+    }
+
+    public static void execute() {
+
+        for (Runnable block : m_blocks) {
+            block.run();
+        }
+    }
+}

src/main/java/frc4388/utility/ReefPositionHelper.java

@@ -63,14 +63,11 @@ public class ReefPositionHelper {
      * Function to find closest tag location based on side
      */
     public static Pose2d getNearestTag(Pose2d position) {
-        Optional<Alliance> ally = DriverStation.getAlliance();
-        if (!ally.isPresent()) 
-            return new Pose2d();
-        if (ally.get() == Alliance.Red) 
+
+        if(TimesNegativeOne.isRed)
             return getNearestTag(RED_TAGS, position);
-        if (ally.get() == Alliance.Blue) 
-            return getNearestTag(BLUE_TAGS, position);   
-        return new Pose2d();
+        else
+            return getNearestTag(BLUE_TAGS, position);
     }
 
     public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) {

src/main/java/frc4388/utility/SlowPeriod.java

@@ -1,6 +0,0 @@
-package frc4388.utility;
-
-// A very stupid class to hold the state of the swerve speed setting for a short period of time
-public class SlowPeriod {
-    
-}

src/main/java/frc4388/utility/TimesNegativeOne.java

@@ -0,0 +1,51 @@
+package frc4388.utility;
+
+import java.util.Optional;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc4388.robot.Constants.SwerveDriveConstants;
+
+// Class that holds weather the drivers sticks should be inverted
+public class TimesNegativeOne {
+
+    public static boolean XAxis = SwerveDriveConstants.INVERT_X;
+    public static boolean YAxis = SwerveDriveConstants.INVERT_Y;
+    public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION;
+    public static boolean isRed = false;
+    public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET);
+
+    private static boolean isRed() {
+        Optional<Alliance> alliance = DriverStation.getAlliance();
+        if(alliance.isEmpty()) return false;
+        return (alliance.get() == Alliance.Red);
+    }
+
+    public static void update(){
+        isRed = isRed();
+        XAxis = SwerveDriveConstants.INVERT_X ^ isRed;
+        YAxis = SwerveDriveConstants.INVERT_Y ^ isRed;
+        RotAxis = SwerveDriveConstants.INVERT_ROTATION;
+        ForwardOffset = Rotation2d.fromDegrees((SwerveDriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0)));
+        SmartDashboard.putBoolean("Is red alliance", isRed);
+    }
+
+    public static double invert(double num, boolean invert){
+        return invert ? -num : num;
+    }
+
+    public static Translation2d invert(Translation2d stick, boolean invertXY){
+        if(invertXY) return stick.times(-1);
+        else return stick;
+    }
+
+    public static Translation2d invert(Translation2d stick, boolean invertX, boolean invertY){
+        return new Translation2d(
+            invert(stick.getX(), invertX), 
+            invert(stick.getY(), invertY)
+        );
+    }
+}

vendordeps/PathplannerLib-2025.2.4.json

@@ -1,7 +1,7 @@
 {
-    "fileName": "PathplannerLib-2025.1.1.json",
+    "fileName": "PathplannerLib-2025.2.4.json",
     "name": "PathplannerLib",
-    "version": "2025.1.1",
+    "version": "2025.2.4",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2025.1.1"
+            "version": "2025.2.4"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2025.1.1",
+            "version": "2025.2.4",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,