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

Improve lidar align accuracy

Browse Source
This commit is contained in:
Michael Mikovsky
2025-02-01 15:52:06 -07:00
parent e379794da1
commit 7fa93e6892
7 changed files with 71 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/LidarAlign.java
+5 -2
View File
@@ -6,6 +6,7 @@ package frc4388.robot.commands;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.Lidar;
import frc4388.robot.subsystems.SwerveDrive;
@@ -56,8 +57,10 @@ public class LidarAlign extends Command {
headedRight = !headedRight;
currentFinderTick *= -1;
}
double relAngle = (Math.round(swerveDrive.getGyroAngle() / 60.d) * 60) + 90; // Relative driving to the side of the reef
double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef
SmartDashboard.putNumber("Rel Angle", relAngle);
SmartDashboard.putNumber("heading", currentHeading);
if (!headedRight) {
swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle));
} else {