auto align tuning changes

grt192 · Feb 13, 2024 · 6118497 · 6118497
1 parent 61930db
commit 6118497
Show file tree

Hide file tree

Showing 8 changed files with 103 additions and 51 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -34,10 +34,10 @@ public static class ElevatorConstants {
 
     public static final int ZERO_LIMIT_ID = 3;
 
-    public static final double GROUND_POSITION = Units.inchesToMeters(1.5);
+    public static final double GROUND_POSITION = Units.inchesToMeters(3);
     public static final double SPEAKER_POSITION = 0;
     public static final double AMP_POSITION = Units.inchesToMeters(29);
-    public static final double CHUTE_POSITION = Units.inchesToMeters(1.5);
+    public static final double CHUTE_POSITION = Units.inchesToMeters(3);
     public static final double TRAP_POSITION = Units.inchesToMeters(30);
     public static final double START_POSITION = 0; 
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -16,7 +16,6 @@
 import frc.robot.controllers.DualJoystickDriveController;
 import frc.robot.controllers.XboxDriveController;
 import frc.robot.commands.IdleCommand;
-import frc.robot.commands.align.AlignCommand;
 import frc.robot.commands.climb.ClimbLowerCommand;
 import frc.robot.commands.climb.ClimbRaiseCommand;
 import frc.robot.commands.elevator.ElevatorToAMPCommand;
@@ -30,6 +29,8 @@
 import frc.robot.commands.shooter.feed.ShooterFeedShootCommand;
 import frc.robot.commands.shooter.pivot.ShooterPivotSetAngleCommand;
 import frc.robot.commands.shooter.pivot.ShooterPivotVerticalCommand;
+import frc.robot.commands.swerve.AlignCommand;
+import frc.robot.commands.swerve.SwerveStopCommand;
 import frc.robot.subsystems.climb.ClimbSubsystem;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.controllers.BaseDriveController;
@@ -155,8 +156,6 @@ public RobotContainer() {
         // module = new SwerveModule(6, 7, 0);
         // baseSwerveSubsystem = new TestSingleModuleSwerveSubsystem(module);
 
-        swerveCrauton.add("AUTO ALIGN BLUE AMP", AlignCommand.getAlignCommand(AutoAlignConstants.BLUE_AMP_POSE));
-
         camera1 = new UsbCamera("camera1", 0);
         camera1.setFPS(60);
         camera1.setBrightness(45);
@@ -193,20 +192,28 @@ private void configureBindings() {
           xButton.onTrue(new IntakeRollerOutakeCommand(intakeRollerSubsystem));
 
         if(baseSwerveSubsystem instanceof SwerveSubsystem){
-          final SwerveSubsystem swerveSubsystem = (SwerveSubsystem) baseSwerveSubsystem;
-
-          ledSubsystem.setDefaultCommand(new RunCommand(() -> {
-            ledSubsystem.setDriverHeading(-swerveSubsystem.getDriverHeading().getRadians());// - swerveSubsystem.getRobotPosition().getRotation().getRadians());
-          }, ledSubsystem));
-
-          swerveSubsystem.setDefaultCommand(new RunCommand(() -> {
-              swerveSubsystem.setDrivePowers(driveController.getForwardPower(), driveController.getLeftPower(), driveController.getRotatePower());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis()));
-          // System.out.println(-controller.getLeftY());
-          xError.setValue(xPID.getPositionError());
-          yError.setValue(yPID.getPositionError());
-          // System.out.println("y: " + yPID.getPositionError());
-              // pivotSubsystem.setFieldPosition(swerveSubsystem.getRobotPosition());
-          }, swerveSubsystem));
+            final SwerveSubsystem swerveSubsystem = (SwerveSubsystem) baseSwerveSubsystem;
+            swerveCrauton.add("AUTO ALIGN BLUE AMP", AlignCommand.getAlignCommand(AutoAlignConstants.BLUE_AMP_POSE, swerveSubsystem));
+
+            ledSubsystem.setDefaultCommand(new RunCommand(() -> {
+                ledSubsystem.setDriverHeading(driveController.getRelativeMode() ? 0 : -swerveSubsystem.getDriverHeading().getRadians());
+            }, ledSubsystem));
+
+            driveController.getAmpAlign().onTrue(AlignCommand.getAlignCommand(AutoAlignConstants.BLUE_AMP_POSE, swerveSubsystem));
+            driveController.getSwerveStop().onTrue(new SwerveStopCommand(swerveSubsystem));
+
+            swerveSubsystem.setDefaultCommand(new RunCommand(() -> {
+                if(driveController.getRelativeMode()){
+                    swerveSubsystem.setRobotRelativeDrivePowers(driveController.getForwardPower(), driveController.getLeftPower(), driveController.getRotatePower());
+                } else {
+                    swerveSubsystem.setDrivePowers(driveController.getForwardPower(), driveController.getLeftPower(), driveController.getRotatePower());
+                }
+                // pivotSubsystem.setFieldPosition(swerveSubsystem.getRobotPosition());
+                xError.setValue(xPID.getPositionError());
+                yError.setValue(yPID.getPositionError());
+                // System.out.println("y: " + yPID.getPositionError());
+                // pivotSubsystem.setFieldPosition(swerveSubsystem.getRobotPosition());
+            }, swerveSubsystem));
 
           driveController.getFieldResetButton().onTrue(new InstantCommand(() -> {
               swerveSubsystem.resetDriverHeading();

diff --git a/src/main/java/frc/robot/commands/align/AlignCommand.java b/src/main/java/frc/robot/commands/align/AlignCommand.java
diff --git a/src/main/java/frc/robot/commands/swerve/AlignCommand.java b/src/main/java/frc/robot/commands/swerve/AlignCommand.java
@@ -0,0 +1,36 @@
+package frc.robot.commands.swerve;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathConstraints;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.swerve.SwerveSubsystem;
+
+public class AlignCommand {
+
+    public AlignCommand(){
+
+
+    }
+
+    static public Command getAlignCommand(Pose2d targetPose, SwerveSubsystem swerveSubsystem){
+        Command command = AutoBuilder.pathfindToPose(
+            targetPose, 
+            new PathConstraints(
+            2.0, 2.0, 
+                    Units.degreesToRadians(720), Units.degreesToRadians(1080)
+                    ), 
+            0, 
+            0.0
+                );
+
+        command.addRequirements(swerveSubsystem);
+
+        return command;
+    }
+
+}
diff --git a/src/main/java/frc/robot/commands/swerve/SwerveStopCommand.java b/src/main/java/frc/robot/commands/swerve/SwerveStopCommand.java
@@ -0,0 +1,21 @@
+package frc.robot.commands.swerve;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.swerve.SwerveSubsystem;
+
+public class SwerveStopCommand extends Command {
+    SwerveSubsystem swerveSubsystem;
+
+    public SwerveStopCommand(SwerveSubsystem swerveSubsystem){
+        addRequirements(swerveSubsystem);
+        this.swerveSubsystem = swerveSubsystem;
+    }
+
+    public void initialize() {
+        swerveSubsystem.setDrivePowers(0, 0, 0);
+    }
+
+    public boolean isFinished() {
+        return true;
+    }
+}
diff --git a/src/main/java/frc/robot/controllers/BaseDriveController.java b/src/main/java/frc/robot/controllers/BaseDriveController.java
@@ -28,4 +28,8 @@ public abstract class BaseDriveController {
     public abstract JoystickButton getRightBumper();
 
     public abstract Boolean getRelativeMode();
+
+    public abstract JoystickButton getAmpAlign();
+
+    public abstract JoystickButton getSwerveStop();
 }
diff --git a/src/main/java/frc/robot/controllers/DualJoystickDriveController.java b/src/main/java/frc/robot/controllers/DualJoystickDriveController.java
@@ -80,4 +80,12 @@ public JoystickButton getLeftBumper(){
     public Boolean getRelativeMode(){
         return rightTrigger.getAsBoolean();
     }
+
+    public JoystickButton getAmpAlign(){
+        return leftTrigger;
+    }
+
+    public JoystickButton getSwerveStop(){
+        return leftMiddleButton;
+    }
 }
diff --git a/src/main/java/frc/robot/controllers/XboxDriveController.java b/src/main/java/frc/robot/controllers/XboxDriveController.java
@@ -51,4 +51,12 @@ public JoystickButton getRightBumper() {
     public Boolean getRelativeMode() {
         return driveController.getRightTriggerAxis() > .1;
     }
+
+    public JoystickButton getAmpAlign() {
+        return driveXButton;
+    }
+
+    public JoystickButton getSwerveStop() {
+        return driveBButton;
+    }
 }