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

x /= i^2

Browse Source
This commit is contained in:
Michael Mikovsky
2025-02-24 17:59:44 -07:00
parent 761a56cf0f
commit 07841b303a
6 changed files with 71 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/GotoLastApril.java
+3 -11
View File
@@ -15,6 +15,7 @@ import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Vision;
import frc4388.utility.Gains;
import frc4388.utility.ReefPositionHelper;
import frc4388.utility.TimesNegativeOne;
import frc4388.utility.ReefPositionHelper.Side;
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
@@ -75,18 +76,9 @@ public class GotoLastApril extends Command {
@Override
public void execute() {
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();
}
xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
yerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.YAxis);
roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
// SmartDashboard.putNumber("PID X Error", xerr);