Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 08:48:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Lidar Test

Browse Source
This commit is contained in:
SHikhar
2026-02-25 17:45:22 -07:00
parent b3b32b00fe
commit 4ca55e0225
3 changed files with 24 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/alignment/AutoAlign.java
+5 -6
View File
@@ -15,14 +15,15 @@ public class AutoAlign extends Command {
private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
private Pose2d targetpos;
private boolean isLidar;
SwerveDrive swerveDrive;
Vision vision;
public AutoAlign(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos) {
public AutoAlign(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos, boolean isLidar) {
this.swerveDrive = swerveDrive;
this.vision = vision;
this.targetpos = targetpos;
this.isLidar = isLidar;
addRequirements(swerveDrive);
}
@@ -39,8 +40,6 @@ public class AutoAlign extends Command {
@Override
public void execute() {
Pose2d robotPose = vision.getPose2d();
if (robotPose == null) return;
@@ -54,9 +53,9 @@ public class AutoAlign extends Command {
if (roterr < -180) roterr += 360;
SmartDashboard.putNumber("PID Rot Error", roterr);
System.out.println("Target: " + targetpos + "Heading: " + desiredHeading + "Error: " + roterr);
swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), desiredHeading);
}
}
@Override
public final boolean isFinished() {