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

Auto align scoring

Browse Source
This commit is contained in:
Michael Mikovsky
2025-02-18 19:39:01 -07:00
parent 47646dc12b
commit 2641abe8a4
7 changed files with 180 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/GotoLastApril.java
+10 -2
View File
@@ -28,15 +28,19 @@ public class GotoLastApril extends Command {
SwerveDrive swerveDrive;
Vision vision;
double distance;
Side side;
/**
* Command to drive robot to position.
* @param SwerveDrive m_robotSwerveDrive
*/
public GotoLastApril(SwerveDrive swerveDrive, Vision vision) {
public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side) {
this.swerveDrive = swerveDrive;
this.vision = vision;
this.distance = distance;
this.side = side;
// addRequirements(swerveDrive);
}
@@ -50,7 +54,7 @@ public class GotoLastApril extends Command {
public void initialize() {
xPID.initialize();
yPID.initialize();
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.LEFT, AutoConstants.X_OFFSET_TRIM.get());
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side, AutoConstants.X_OFFSET_TRIM.get(), distance);
}
double xerr;
@@ -70,6 +74,10 @@ public class GotoLastApril extends Command {
double youtput = yPID.update(yerr);
double rotoutput = rotPID.update(roterr);
xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
Translation2d leftStick = new Translation2d(
Math.max(Math.min(-youtput, 1), -1),
Math.max(Math.min(-xoutput, 1), -1)