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

fix: excise ballerinas from robot code

Browse Source
This commit is contained in:
C4llSiqn
2025-02-25 19:49:10 -07:00
parent 11861bf580
commit 57a9435c5d
10 changed files with 70 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/GotoLastApril.java
+18 -2
View File
@@ -77,9 +77,25 @@ public class GotoLastApril extends Command {
@Override
public void execute() {
xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
yerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.YAxis);
yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis);
// xerr = targetpos.getX() - vision.getPose2d().getX();
// yerr = targetpos.getX() - vision.getPose2d().getY();
roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
// roterr = TimesNegativeOne.invert(targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(), TimesNegativeOne.isRed);
roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees()));
boolean invert = Math.abs(roterr) > 180;
if(roterr > 180){
roterr -= 360;
}else if(roterr < -180){
roterr += 360;
}
SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
SmartDashboard.putNumber("Rotational PID error", roterr);
// SmartDashboard.putNumber("PID X Error", xerr);
// SmartDashboard.putNumber("PID Y Error", yerr);