-
Notifications
You must be signed in to change notification settings - Fork 12
/
HeadingLockedFieldRelativeController.java
31 lines (27 loc) · 1.58 KB
/
HeadingLockedFieldRelativeController.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
package com.team254.lib.swerve;
import com.team254.frc2022.Constants;
import com.team254.lib.control.RadiusController;
import com.team254.lib.control.SwerveHeadingController;
import com.team254.lib.geometry.Pose2d;
public class HeadingLockedFieldRelativeController implements IDriveController {
public static HeadingLockedFieldRelativeController mInstance;
public SwerveHeadingController mSwerveHeadingController = SwerveHeadingController.getInstance();
public RadiusController mRadiusController = RadiusController.getInstance();
public static HeadingLockedFieldRelativeController getInstance() {
if (mInstance == null) {
mInstance = new HeadingLockedFieldRelativeController();
}
return mInstance;
}
@Override
public ChassisSpeeds transform(DriveInput driveInput, Pose2d robotPose) {
mRadiusController.setRadiusControllerState(RadiusController.RadiusControllerState.OFF);
mSwerveHeadingController.setHeadingControllerState(SwerveHeadingController.HeadingControllerState.SNAP);
mSwerveHeadingController.setGoal(driveInput.getDesiredCardinalHeading());
return ChassisSpeeds.fromFieldRelativeSpeeds(
driveInput.getThrottle() * Constants.kMaxVelocityMetersPerSecond * Constants.kScaleTranslationInputs,
driveInput.getStrafe() * Constants.kMaxVelocityMetersPerSecond * Constants.kScaleTranslationInputs,
mSwerveHeadingController.update(robotPose.getRotation().getDegrees()) * Constants.kMaxAngularVelocityRadiansPerSecond,
robotPose.getRotation());
}
}