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

Add trims and get reef position

Browse Source
This commit is contained in:
Michael Mikovsky
2025-01-16 19:41:05 -07:00
parent 56f3ccf0c5
commit 9db9d25f5b
6 changed files with 64 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/GotoPositionCommand.java
+4 -2
View File
@@ -9,6 +9,8 @@ import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Vision;
import frc4388.utility.Gains;
import frc4388.utility.ReefPositionHelper;
import frc4388.utility.ReefPositionHelper.Side;
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
import frc4388.utility.controller.VirtualController;
@@ -32,10 +34,9 @@ public class GotoPositionCommand extends Command {
* @param SwerveDrive m_robotSwerveDrive
*/
public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos) {
public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) {
this.swerveDrive = swerveDrive;
this.vision = vision;
this.targetpos = targetpos;
addRequirements(swerveDrive);
}
@@ -43,6 +44,7 @@ public class GotoPositionCommand extends Command {
public void initialize() {
xPID.initialize();
yPID.initialize();
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.RIGHT, AutoConstants.X_OFFSET_TRIM.get());
}
double xerr;