After-cougars-scrimmage

src/main/deploy/pathplanner/autos/Line-up-no-arm.auto

@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Top Blue Cage to Reef"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "lineup-no-arm"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path

@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 7.270707070707072,
+        "x": 6.599873737373737,
-        "y": 7.013257575757576
+        "y": 8.029671717171716
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.309027777777779,
+        "x": 5.464431818181818,
-        "y": 5.3768308080808085
+        "y": 5.6403977272727275
       },
       "prevControl": {
-        "x": 4.945156854248459,
+        "x": 5.240820707070706,
-        "y": 5.08761998976538
+        "y": 5.254160353535354
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 60.255118703057796
+    "rotation": -122.99770510121631
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -2.223961240385466
+    "rotation": 180.0
   },
   "useDefaultConstraints": true
 }

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

@@ -117,38 +117,48 @@ public class RobotContainer {
     //        true, false);
     // private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
     private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
+        new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
+
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+
         new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
-        // new InstantCommand(() -> System.out.println("Soup")),
+
-        // new WaitCommand(1),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1 - Units.inchesToMeters(0), Side.RIGHT),
+
+        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.RIGHT),
         new LidarAlign(m_robotSwerveDrive, m_lidar),
+
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
+
         new waitEndefectorRefrence(m_robotElevator),
+
         new MoveForTimeCommand(m_robotSwerveDrive, 
         new Translation2d(0,1), new Translation2d(), 500, true),
+        
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
     );
 
     private Command AprilLidarAlignL4Left = new SequentialCommandGroup(
+        new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
         new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
         // new InstantCommand(() -> System.out.println("Soup")),
         // new WaitCommand(1),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT),
+
+        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.LEFT),
         new LidarAlign(m_robotSwerveDrive, m_lidar),
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
-        new Translation2d(0,1), new Translation2d(), 500, true),
+            new Translation2d(0,1), new Translation2d(), 500, true),
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
     );
 
     private Command AprilLidarAlignL3Left = new SequentialCommandGroup(
+        new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
         new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT),
         // new InstantCommand(() -> System.out.println("Soup")),
@@ -166,6 +176,7 @@ public class RobotContainer {
     );
 
     private Command AprilLidarAlignL3Right = new SequentialCommandGroup(
+        new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
 
         new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
@@ -187,6 +198,7 @@ public class RobotContainer {
     );
     
     private Command AprilLidarAlignL2Left = new SequentialCommandGroup(
+        new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
         new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.LEFT),
         
         new LidarAlign(m_robotSwerveDrive, m_lidar),
@@ -200,6 +212,7 @@ public class RobotContainer {
     );
 
     private Command AprilLidarAlignL2Right = new SequentialCommandGroup(
+        new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
         new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.RIGHT),
         
         new LidarAlign(m_robotSwerveDrive, m_lidar),
@@ -231,6 +244,7 @@ public class RobotContainer {
     );
 
     private Command leftAlgaeRemove = new SequentialCommandGroup(
+        new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
@@ -243,6 +257,7 @@ public class RobotContainer {
     );
 
     private Command rightAlgaeRemove = new SequentialCommandGroup(
+        new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
@@ -261,9 +276,18 @@ public class RobotContainer {
      */
     public RobotContainer() {
         
-        NamedCommands.registerCommand("AutoGotoPosition", AprilLidarAlignL4Right);
+        NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
-        NamedCommands.registerCommand("april-allign", AprilLidarAlignL4Right);
+        NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right);
-        NamedCommands.registerCommand("place-coral", placeCoral);
+        NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL3Left);
+        NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL3Right);
+        NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL2Left);
+        NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL2Right);
+
+        NamedCommands.registerCommand("lineup-no-arm", new SequentialCommandGroup(
+            new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
+            new LidarAlign(m_robotSwerveDrive, m_lidar)
+        ));
+
         NamedCommands.registerCommand("grab-coral", grabCoral);
 
         configureButtonBindings();        
@@ -333,10 +357,6 @@ public class RobotContainer {
      */
     private void configureButtonBindings() {
 
-        // ? /* Test button box bindings */
-        new JoystickButton(getButtonBox(), ButtonBox.White)
-            .onTrue(new InstantCommand(() -> System.out.println("White!")));
-
         // ? /* Driver Buttons */
         DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
@@ -360,18 +380,18 @@ public class RobotContainer {
 
 
 
-        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
+        new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
-            .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepUp()));
+            .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp()));
 
-        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180)
+        new Trigger(() -> getDeadbandedDriverController().getPOV() == 180)
-            .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown()));
+            .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown()));
 
+        new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
+            .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp()));
 
-        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90)
+        new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
-            .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp()));
+            .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
 
-        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270)
-            .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepDown()));
 
 
 
@@ -419,23 +439,35 @@ public class RobotContainer {
         new JoystickButton(getButtonBox(), ButtonBox.Four)
             .onTrue(rightAlgaeRemove);
         
+
+        // Cancel button
         new JoystickButton(getButtonBox(), ButtonBox.White)
             .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endefectorStop();}, m_robotElevator));
 
+        // Score prep        
+        // Prime 4
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
+            .onTrue(new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator));
+
+        // Prime 3
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
+            .onTrue(new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator));
+
+        
+
         //Trims
-        new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
+        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
-            .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp()));
+            .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepUp()));
 
-        new Trigger(() -> getDeadbandedDriverController().getPOV() == 180)
+        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180)
-            .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown()));
+            .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown()));
 
 
-        new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
+        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90)
-            .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp()));
+            .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp()));
-
-        new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
-            .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
 
+        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270)
+            .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepDown()));
             
         // ? /* Programer Buttons (Controller 3)*/
 
@@ -488,16 +520,18 @@ public class RobotContainer {
      */
     public Command getAutonomousCommand() {
 
+
         //return autoPlayback;
         //return new GotoPositionCommand(m_robotSwerveDrive, m_vision)
         //return autoChooser.getSelected();
 	// try{
-	//     // Load the path you want to follow using its name in the GUI
+	// //     // Load the path you want to follow using its name in the GUI
-	    return autoCommand;
+    //     return autoCommand;
 	// } catch (Exception e) {
 	//     DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
-	//     return Commands.none();
+	    return Commands.none();
 	// }
+    // return new PathPlannerAuto("Line-up-no-arm");
 	// zach told me to do the below comment
 	//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
       //  return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);

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

@@ -1,9 +1,13 @@
 package frc4388.robot.commands;
 
+import java.util.Optional;
+
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
@@ -51,6 +55,8 @@ public class GotoLastApril extends Command {
         tagRelativeXError = val;
     }
 
+    Alliance alliance = null;
+
     @Override
     public void initialize() {
         xPID.initialize();
@@ -58,6 +64,9 @@ public class GotoLastApril extends Command {
         this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side, 
         Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()), 
         distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get()));
+        Optional<Alliance> a = DriverStation.getAlliance();
+        if(!a.isEmpty())
+            alliance = a.get();
     }
     
     double xerr;
@@ -66,8 +75,18 @@ public class GotoLastApril extends Command {
 
     @Override
     public void execute() {
-        xerr = targetpos.getX() - vision.getPose2d().getX();
+        
-        yerr = targetpos.getY() - vision.getPose2d().getY();
+        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();
+        }
+
+            
         roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
 
         // SmartDashboard.putNumber("PID X Error", xerr);
@@ -81,7 +100,7 @@ public class GotoLastApril extends Command {
         youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
         rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
         
-        // System.out.println(xerr + ", " + yerr + ", " + roterr + ", " + rotoutput);
+
 
         Translation2d leftStick = new Translation2d(
             Math.max(Math.min(-youtput, 1), -1),
@@ -94,6 +113,8 @@ public class GotoLastApril extends Command {
            0
         );
 
+
+
         setTagRelativeXError(
             new Translation2d(xerr, yerr).getAngle()
             .rotateBy(targetpos.getRotation())

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

@@ -227,7 +227,7 @@ public class Elevator extends SubsystemBase {
     SmartDashboard.putString("State", currentState.toString());
 
     if (currentState == CoordinationState.Waiting) {
-      periodicWaiting();
+      // periodicWaiting();
     } else if (currentState == CoordinationState.WatingBeamTriped) {
       // periodicWaitingTripped();
     } else if (currentState == CoordinationState.Ready) {

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

@@ -142,9 +142,19 @@ public class SwerveDrive extends Subsystem {
 
         leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
 
-        if (SwerveDriveConstants.INVERT_X) leftStick = new Translation2d(-leftStick.getX(), leftStick.getY());
+        Optional<Alliance> alliance = DriverStation.getAlliance();
-        if (SwerveDriveConstants.INVERT_Y) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
+        
+        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);
+
+
+        
         stopped = false;
         if (fieldRelative) {
             // ! drift correction