Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

make lidar align theoretically work

Browse Source
This commit is contained in:
C4llSiqn
2025-01-29 17:06:55 -07:00
parent 2bc1d6eb9c
commit 42e7896f5f
3 changed files with 65 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+25
View File
@@ -210,6 +210,21 @@ public class SwerveDrive extends Subsystem {
.withTargetDirection(rightStick.getAngle()));
}
public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
leftStick = leftStick.rotateBy(heading);
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(Rotation2d.fromDegrees(rotTarget));
ctrl.HeadingController.setPID(
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
);
swerveDriveTrain.setControl(ctrl);
}
public boolean rotateToTarget(double angle) {
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(0)
@@ -248,6 +263,16 @@ public class SwerveDrive extends Subsystem {
swerveDriveTrain.tareEverything();
}
public void softStop() {
stopped = true;
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
.withVelocityX(0)
.withVelocityY(0)
.withRotationalRate(0)
); // stop the modules without breaking
}
public void stopModules() {
stopped = true;
swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());