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

l2 baby

Browse Source
This commit is contained in:
C4llSiqn
2025-02-21 20:22:07 -07:00
parent c9a947d3b8
commit 0914601699
4 changed files with 108 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/GotoLastApril.java
+4 -1
View File
@@ -3,6 +3,7 @@ package frc4388.robot.commands;
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.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
@@ -54,7 +55,9 @@ public class GotoLastApril extends Command {
public void initialize() {
xPID.initialize();
yPID.initialize();
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side, AutoConstants.X_OFFSET_TRIM.get(), distance);
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;