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

April align now uses robot oriented

Browse Source
This commit is contained in:
Michael Mikovsky
2025-03-05 12:15:45 -07:00
parent 7dc6105145
commit 357b054c86
3 changed files with 19 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+1 -1
View File
@@ -297,7 +297,7 @@ public final class Constants {
public static final Gains XY_GAINS = new Gains(5,0.4,0.0);
// public static final Gains XY_GAINS = new Gains(3,0.3,0.0);
public static final Gains ROT_GAINS = new Gains(0.05,0,0.007);
// public static final Gains ROT_GAINS = new Gains(0.05,0,0.007);
// public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
src/main/java/frc4388/robot/commands/GotoLastApril.java
+17 -21
View File
@@ -31,7 +31,7 @@ public class GotoLastApril extends Command {
private PID xPID = new PID(AutoConstants.XY_GAINS, 0);
private PID yPID = new PID(AutoConstants.XY_GAINS, 0);
private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
// private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
private Pose2d targetpos;
SwerveDrive swerveDrive;
@@ -58,18 +58,16 @@ public class GotoLastApril extends Command {
tagRelativeXError = val;
}
Alliance alliance = null;
@Override
public void initialize() {
xPID.initialize();
yPID.initialize();
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side,
Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()),
distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get()));
Optional<Alliance> a = DriverStation.getAlliance();
if(!a.isEmpty())
alliance = a.get();
this.targetpos = ReefPositionHelper.getNearestPosition(
this.vision.getPose2d(),
side,
Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()),
distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get())
);
}
double xerr;
@@ -91,8 +89,6 @@ public class GotoLastApril extends Command {
roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees()));
boolean invert = Math.abs(roterr) > 180;
if(roterr > 180){
roterr -= 360;
}else if(roterr < -180){
@@ -108,11 +104,11 @@ public class GotoLastApril extends Command {
xoutput = xPID.update(xerr);
youtput = yPID.update(yerr);
rotoutput = rotPID.update(roterr);
// 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;
// rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
@@ -122,19 +118,18 @@ public class GotoLastApril extends Command {
// 0,0
);
Translation2d rightStick = new Translation2d(
Math.max(Math.min(rotoutput, 1), -1),
0
);
// Translation2d rightStick = new Translation2d(
// Math.max(Math.min(rotoutput, 1), -1),
// 0
// );
setTagRelativeXError(
new Translation2d(xerr, yerr).getAngle()
.rotateBy(targetpos.getRotation())
.getCos());
swerveDrive.driveWithInput(leftStick, rightStick, true);
swerveDrive.driveRelativeLockedAngle(leftStick, targetpos.getRotation());
// swerveDrive.driveWithInputOrientation(leftStick, rightStick, true);
}
@Override
@@ -142,7 +137,8 @@ public class GotoLastApril extends Command {
boolean finished = (
(Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) &&
(Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) &&
(Math.abs(roterr) < AutoConstants.ROT_TOLERANCE || Math.abs(rotoutput) <= AutoConstants.MIN_ROT_PID_OUTPUT));
(Math.abs(roterr) < AutoConstants.ROT_TOLERANCE)
);
// System.out.println(finished);
if(finished)
src/main/java/frc4388/robot/commands/LidarAlign.java
+1 -1
View File
@@ -97,7 +97,7 @@ public class LidarAlign extends Command {
currentFinderTick = 0;
foundReef = false;
return false;
} else if (bounces == 2) {
} else if (bounces >= 2) {
swerveDrive.stopModules();
return true;
} else {