absolute rotation combined with trigger turning

Author: gw12343

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

@@ -8,6 +8,7 @@ public static final class ControllerConstants {
 		public static final int kDriverControllerPort = 0;
 		public static final int kOperatorControllerPort = 1;
 		public static final double kDeadzone = 0.05;
+		public static final double kAbsoluteTurnDeadzone = 0.5;
 		public static final double kTriggerDeadzone = .05;
 	}
 
@@ -36,6 +37,7 @@ public static final class DriveConstants {
 
 		public static final double kTeleopMaxVoltage = 12;
 		public static final double kTeleopMaxTurnVoltage = 7.2;
+		public static final double kTurnSpeedMultiplier = 3;
 		public static final double kGearRatio = 6.12;
 		public static final double kWheelDiameter = Units.inchesToMeters(4);
 		public static final double kWheelCircumference = Math.PI * kWheelDiameter;

src/main/java/frc/robot/Robot.java

@@ -44,8 +44,11 @@ public void bindDriveControls() {
 				m_driveSubsystem.driveCommand(
 						() -> -m_driverController.getLeftY(),
 						() -> -m_driverController.getLeftX(),
-						() -> m_driverController.getR2Axis() - m_driverController.getL2Axis(),
+						() -> m_driverController.getL2Axis() - m_driverController.getR2Axis(),
+						() -> -m_driverController.getRightX(),
+						() -> -m_driverController.getRightY(),
 						m_driverController.getHID()::getSquareButton));
+		m_driverController.triangle().onTrue(m_driveSubsystem.resetHeadingCommand());
 	}
 
 	@Override

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

@@ -14,8 +14,10 @@
 import com.studica.frc.AHRS.NavXComType;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
 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.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
@@ -46,9 +48,12 @@ public class DriveSubsystem extends SubsystemBase {
 	private final StructPublisher<Pose2d> m_posePublisher;
 	private final StructArrayPublisher<SwerveModuleState> m_targetModuleStatePublisher;
 	private final StructArrayPublisher<SwerveModuleState> m_currentModuleStatePublisher;
+	private final PIDController m_absoluteAngleController;
 
 	/** Creates a new DriveSubsystem. */
 	public DriveSubsystem() {
+		m_absoluteAngleController = new PIDController(5, 0, 0);
+		m_absoluteAngleController.enableContinuousInput(-Math.PI, Math.PI);
 		m_posePublisher = NetworkTableInstance.getDefault().getStructTopic("/SmartDashboard/Pose", Pose2d.struct)
 				.publish();
 		m_targetModuleStatePublisher = NetworkTableInstance.getDefault()
@@ -190,20 +195,29 @@ public void periodic() {
 	 * @return A command to drive the robot.
 	 */
 	public Command driveCommand(DoubleSupplier forwardSpeed, DoubleSupplier strafeSpeed,
-			DoubleSupplier rotation, BooleanSupplier isFieldRelative) {
+			DoubleSupplier rotation, DoubleSupplier absoluteRotationForward, DoubleSupplier absoluteRotationStrafe,
+			BooleanSupplier isFieldRelative) {
 		return run(() -> {
+			Translation2d t = new Translation2d(absoluteRotationStrafe.getAsDouble(),
+					absoluteRotationForward.getAsDouble());
+
 			// Get the forward, strafe, and rotation speed, using a deadband on the joystick
 			// input so slight movements don't move the robot
 			double rotSpeed = MathUtil.applyDeadband(rotation.getAsDouble(), ControllerConstants.kDeadzone);
-			rotSpeed = Math.signum(rotSpeed) * Math.pow(rotSpeed, 2) * kTeleopMaxTurnVoltage;
+			double threshold = Math.max(ControllerConstants.kAbsoluteTurnDeadzone, rotSpeed);
+			rotSpeed *= kTurnSpeedMultiplier;
+
+			if (t.getNorm() > threshold)
+				rotSpeed = m_absoluteAngleController.calculate(getHeading().getRadians(), t.getAngle().getRadians());
+			rotSpeed = Math.signum(rotSpeed) * Math.min(Math.abs(rotSpeed), kTeleopMaxTurnVoltage);
 
 			double fwdSpeed = MathUtil.applyDeadband(forwardSpeed.getAsDouble(), ControllerConstants.kDeadzone);
 			fwdSpeed = Math.signum(fwdSpeed) * Math.pow(fwdSpeed, 2) * kTeleopMaxVoltage;
 
 			double strSpeed = MathUtil.applyDeadband(strafeSpeed.getAsDouble(), ControllerConstants.kDeadzone);
 			strSpeed = Math.signum(strSpeed) * Math.pow(strSpeed, 2) * kTeleopMaxVoltage;
 
-			drive(fwdSpeed, strSpeed, rotSpeed, isFieldRelative.getAsBoolean());
+			drive(fwdSpeed, strSpeed, rotSpeed, !isFieldRelative.getAsBoolean());
 		}).withName("DefaultDriveCommand");
 	}