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

After-cougars-scrimmage

Browse Source
This commit is contained in:
Michael Mikovsky
2025-02-24 17:10:12 -07:00
parent 460d4fbbd4
commit 761a56cf0f
6 changed files with 137 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/GotoLastApril.java
+24 -3
View File
@@ -1,9 +1,13 @@
package frc4388.robot.commands;
import java.util.Optional;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
@@ -51,6 +55,8 @@ public class GotoLastApril extends Command {
tagRelativeXError = val;
}
Alliance alliance = null;
@Override
public void initialize() {
xPID.initialize();
@@ -58,6 +64,9 @@ public class GotoLastApril extends Command {
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();
}
double xerr;
@@ -66,8 +75,18 @@ public class GotoLastApril extends Command {
@Override
public void execute() {
xerr = targetpos.getX() - vision.getPose2d().getX();
yerr = targetpos.getY() - vision.getPose2d().getY();
if (alliance == Alliance.Red) {
xerr = -(targetpos.getX() - vision.getPose2d().getX());
yerr = (targetpos.getY() - vision.getPose2d().getY());
}
else{
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);
@@ -80,8 +99,8 @@ public class GotoLastApril extends Command {
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;
// System.out.println(xerr + ", " + yerr + ", " + roterr + ", " + rotoutput);
Translation2d leftStick = new Translation2d(
Math.max(Math.min(-youtput, 1), -1),
@@ -94,6 +113,8 @@ public class GotoLastApril extends Command {
0
);
setTagRelativeXError(
new Translation2d(xerr, yerr).getAngle()
.rotateBy(targetpos.getRotation())