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

Debug autos

Browse Source
This commit is contained in:
Michael Mikovsky
2025-01-25 14:47:23 -07:00
parent a7ac44c25d
commit 1b9b7ed00c
23 changed files with 272 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/GotoPositionCommand.java
+5 -3
View File
@@ -37,14 +37,14 @@ public class GotoPositionCommand extends Command {
public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) {
this.swerveDrive = swerveDrive;
this.vision = vision;
addRequirements(swerveDrive);
// addRequirements(swerveDrive);
}
@Override
public void initialize() {
xPID.initialize();
yPID.initialize();
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.RIGHT, AutoConstants.X_OFFSET_TRIM.get());
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.LEFT, AutoConstants.X_OFFSET_TRIM.get());
}
double xerr;
@@ -84,7 +84,9 @@ public class GotoPositionCommand extends Command {
@Override
public final boolean isFinished() {
return (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE);
boolean finished = (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE);
// System.out.println(finished);
return finished;
// this statement is a boolean in and of itself
}