Automatic Align Player Station

simgui-ds.json

@@ -88,5 +88,11 @@
       "buttonCount": 0,
       "povCount": 0
     }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
+    }
   ]
 }

simgui.json

@@ -2,6 +2,10 @@
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
+      "/Shuffleboard/Auto/Auto Location": "String Chooser",
+      "/Shuffleboard/Auto/Auto Path Action": "String Chooser",
+      "/Shuffleboard/Auto/Auto Start Action": "String Chooser",
+      "/Shuffleboard/Auto/Current Auto Trajectory (Always appears on blue side)": "Field2d",
       "/SmartDashboard/Field Sim": "Field2d"
     },
     "windows": {
@@ -22,6 +26,9 @@
   "NetworkTables": {
     "Persistent Values": {
       "open": false
+    },
+    "Retained Values": {
+      "open": false
     }
   }
 }

src/main/java/frc/robot/Constants.java

@@ -4,6 +4,9 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+
 public final class Constants {
       //auto constants
       public static final double AUTO_MAX_VELOCITY = 3;
@@ -21,6 +24,10 @@ public final class Constants {
 
       public static final double FIELD_WIDTH_METERS = 16;
       public static final double FIELD_HEIGHT_METERS = 7.85;
+
+      public static final Pose2d HUMAN_STATION_BOUNDARY = new Pose2d(12.6, 2.5, Rotation2d.fromDegrees(0));
+
+
 
       //sim constants
       public static final double RED_ORIGIN_POS_X_METERS = 16.541748984;

src/main/java/frc/robot/RobotContainer.java

@@ -21,6 +21,7 @@
 import frc.robot.commands.intake.armIntake.IntakeCone;
 import frc.robot.commands.intake.floor.FloorDrop;
 import frc.robot.commands.intake.floor.FloorPickup;
+import frc.robot.commands.vision.PlayerStationAutoAlign;
 import frc.robot.commands.vision.ChangeVisionPipeline;
 import frc.robot.commands.vision.ToggleLED;
 import frc.robot.subsystems.ArmSubsystem;
@@ -68,6 +69,11 @@ public class RobotContainer {
   private final DPadButton operatorDPadLeft = new DPadButton(operatorController, DPadButton.Direction.LEFT);
   private final DPadButton operatorDPadDown = new DPadButton(operatorController, DPadButton.Direction.DOWN);
 
+  private final DPadButton driverDPadUp = new DPadButton(driverController, DPadButton.Direction.UP);
+  private final DPadButton driverDPadRight = new DPadButton(driverController, DPadButton.Direction.RIGHT);
+  private final DPadButton driverDPadLeft = new DPadButton(driverController, DPadButton.Direction.LEFT);
+  private final DPadButton driverDPadDown = new DPadButton(driverController, DPadButton.Direction.DOWN);
+
   public RobotContainer() {
     configureBindings();
   }
@@ -193,6 +199,7 @@ private void configureBindings() {
         );
         //vision
         operatorControllerStartButton.toggleOnTrue(new ToggleLED(vision));
+        driverDPadUp.onTrue(new PlayerStationAutoAlign(drivetrainSubsystem, vision));
         operatorControllerBackButton.toggleOnTrue(new ChangeVisionPipeline(vision));
     }
   }

src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java

@@ -0,0 +1,58 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.vision;
+
+import frc.robot.Constants;
+import frc.robot.subsystems.DrivetrainSubsystem;
+import frc.robot.subsystems.Vision;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class PlayerStationAutoAlign extends CommandBase {
+  /** Creates a new PlayerStationAutoAlign. */
+  private final Vision vision;
+  private final DrivetrainSubsystem drivetrainSubsystem;
+  public PlayerStationAutoAlign(DrivetrainSubsystem drivetrainSubsystem, Vision vision) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.vision = vision;
+    this.drivetrainSubsystem = drivetrainSubsystem;
+    addRequirements(this.drivetrainSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    if(vision.getTargetID() == 4){ // Override if Blue Alliance Human Player April Tag is seen
+      drivetrainSubsystem.resetPose(vision.getBotpose());
+      drivetrainSubsystem.setRotationTarget(Constants.HUMAN_STATION_BOUNDARY.getRotation());
+    }else if(vision.getTargetID() == 5){ // Override if RED Alliance Human Player April Tag is seen
+      drivetrainSubsystem.resetPose(vision.getBotpose());
+      drivetrainSubsystem.setRotationTarget(Constants.HUMAN_STATION_BOUNDARY.getRotation());
+    }else if(DriverStation.getAlliance() == Alliance.Red){ // For Red Alliance
+      if(drivetrainSubsystem.getPose().getX() > Constants.HUMAN_STATION_BOUNDARY.getX() && drivetrainSubsystem.getPose().getY() < Constants.HUMAN_STATION_BOUNDARY.getY()){
+        drivetrainSubsystem.setRotationTarget(Constants.HUMAN_STATION_BOUNDARY.getRotation());
+      }
+    }else{ // For Blue Alliance
+      if(drivetrainSubsystem.getPose().getX() > Constants.HUMAN_STATION_BOUNDARY.getX() && drivetrainSubsystem.getPose().getY() > Constants.HUMAN_STATION_BOUNDARY.getY()){
+        drivetrainSubsystem.setRotationTarget(Constants.HUMAN_STATION_BOUNDARY.getRotation());
+      }
+    }
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

src/main/java/frc/robot/subsystems/Vision.java

@@ -3,6 +3,7 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package frc.robot.subsystems;
+
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.networktables.NetworkTable;