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

Fix limit switch and maybe autos

Browse Source
This commit is contained in:
Michael Mikovsky
2025-02-27 19:45:18 -07:00
parent 1b90bc8289
commit a063f4b309
5 changed files with 42 additions and 27 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
@@ -22,6 +22,7 @@ public class LidarAlign extends Command {
private boolean headedRight;
private double speed;
private int bounces;
private double additionalDistance = 0;
// private final boolean constructedHeadedRight;
/** Creates a new LidarAlign. */
@@ -41,6 +42,7 @@ public class LidarAlign extends Command {
this.speed = 0.4; // TODO: find good speed for this
this.foundReef = false;
this.headedRight = !(GotoLastApril.tagRelativeXError < 0);
this.additionalDistance = 0;
this.bounces = 0;
}
@@ -54,11 +56,12 @@ public class LidarAlign extends Command {
return;
}
if (currentFinderTick > 10) { //arbutrary threshhold for now.
if (currentFinderTick > (15 + additionalDistance)) { //arbutrary threshhold for now.
headedRight = !headedRight;
currentFinderTick *= -1;
bounces++;
if (bounces == 2) return;
additionalDistance += 5;
if (bounces == 5) return;
}
double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef