Keep yaw with AutoDriveTo

redshiftrobotics · Mar 15, 2024 · 75c2353 · 75c2353
1 parent 3a9c087
commit 75c2353
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/commands/AimAtTag.java b/src/main/java/frc/robot/commands/AimAtTag.java
@@ -40,7 +40,7 @@ public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, int tagID, ChassisDr
 				RobotMovementConstants.ROTATION_PID_P,
 				RobotMovementConstants.ROTATION_PID_I,
 				RobotMovementConstants.ROTATION_PID_D);
-		rotatePID.enableContinuousInput(-1, 1);
+		rotatePID.enableContinuousInput(-Math.PI, Math.PI);
 		rotatePID.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
 		rotatePID.setSetpoint(0);
 

diff --git a/src/main/java/frc/robot/commands/AutoDriveTo.java b/src/main/java/frc/robot/commands/AutoDriveTo.java
@@ -12,7 +12,7 @@
 public class AutoDriveTo extends Command {
 	private final SwerveDrivetrain drivetrain;
 
-	private final PIDController xMovePID, yMovePID;
+	private final PIDController xMovePID, yMovePID, rotatePID;
 	private double initX, initY, goalX, goalY;
 
 	/***
@@ -39,6 +39,13 @@ public AutoDriveTo(SwerveDrivetrain subsystem, Translation2d translation) {
 				RobotMovementConstants.TRANSLATION_PID_D);
 		yMovePID.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
 
+		rotatePID = new PIDController(
+				RobotMovementConstants.ROTATION_PID_P,
+				RobotMovementConstants.ROTATION_PID_I,
+				RobotMovementConstants.ROTATION_PID_D);
+		rotatePID.enableContinuousInput(-Math.PI, Math.PI);
+		rotatePID.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
+
 		addRequirements(this.drivetrain);
 	}
 
@@ -49,6 +56,8 @@ public void initialize() {
 
 		final Pose2d position = drivetrain.getPosition();
 
+		rotatePID.setSetpoint(drivetrain.getHeading().getRadians());
+
 		initX = position.getX();
 		initY = position.getY();
 	}
@@ -59,6 +68,7 @@ public void execute() {
 
 		double targetX = position.getX() - initX;
 		double targetY = position.getY() - initY;
+		double targetRadians = drivetrain.getHeading().getRadians();
 
 		double xSpeed = xMovePID.calculate(targetX, goalX);
 		double ySpeed = yMovePID.calculate(targetY, goalY);
@@ -76,7 +86,7 @@ public void execute() {
 			ySpeed = (ySpeed / maxSpeed) * speedLimit;
 		}
 
-		drivetrain.setDesiredState(new ChassisSpeeds(xSpeed, ySpeed, 0));
+		drivetrain.setDesiredState(new ChassisSpeeds(xSpeed, ySpeed, rotatePID.calculate(targetRadians)));
 		drivetrain.updateSmartDashboard();
 	}