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

Y auto activation

Browse Source 
autos activate when y is pressed, automatically stop when done
This commit is contained in:
McGrathMaggie
2025-01-13 19:43:35 -07:00
parent 7fdb5dcd58
commit 882acc9b93
2 changed files with 23 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/GotoPositionCommand.java
+16 -3
View File
@@ -46,11 +46,17 @@ public class GotoPositionCommand extends Command {
yPID.initialize();
}
double xerr;
double yerr;
double roterr;
double xytolerance = 0.01;
double rottolerance = 1;
@Override
public void execute() {
double xerr = targetpos.getX() - vision.getPose2d().getX();
double yerr = targetpos.getY() - vision.getPose2d().getY();
double roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
xerr = targetpos.getX() - vision.getPose2d().getX();
yerr = targetpos.getY() - vision.getPose2d().getY();
roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
SmartDashboard.putNumber("PID X Error", xerr);
SmartDashboard.putNumber("PID Y Error", yerr);
@@ -76,6 +82,13 @@ public class GotoPositionCommand extends Command {
swerveDrive.driveWithInput(leftStick, rightStick, true);
}
@Override
public final boolean isFinished() {
return (Math.abs(xerr) < xytolerance && Math.abs(yerr) < xytolerance && Math.abs(roterr) < rottolerance);
// this statement is a boolean in and of itself
}
// @Override
// public void end(boolean interrupted) {