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

lidar align works

Browse Source
This commit is contained in:
Michael Mikovsky
2025-01-30 19:28:46 -07:00
parent 4afbf1656f
commit e379794da1
5 changed files with 22 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/LidarAlign.java
+4 -4
View File
@@ -37,7 +37,7 @@ public class LidarAlign extends Command {
@Override
public void initialize() {
this.currentFinderTick = 0;
this.speed = 0.1; // TODO: find good speed for this
this.speed = 0.4; // TODO: find good speed for this
this.foundReef = false;
this.headedRight = constructedHeadedRight;
}
@@ -57,11 +57,11 @@ public class LidarAlign extends Command {
currentFinderTick *= -1;
}
double relAngle = Math.round(swerveDrive.getGyroAngle() / 60.d) * 60; // Relative driving to the side of the reef
double relAngle = (Math.round(swerveDrive.getGyroAngle() / 60.d) * 60) + 90; // Relative driving to the side of the reef
if (!headedRight) {
swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle));
} else {
swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle));
} else {
swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle));
}
currentFinderTick++;