sdesharnais/Rebuilt-2026
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

mode auto

Browse Source
This commit is contained in:
samuel desharnais
2026-03-31 17:53:41 -04:00
parent 6afc342006
commit 10b0110315
12 changed files with 190 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java
View File

@@ -52,13 +52,18 @@ public class TournerVersReservoir extends Command {
angle = 0;
}
}
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 10){
drivetrain.setControl(drive.withRotationalRate(0));
}
else{
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){
drivetrain.setControl(drive.withRotationalRate(-force*180/Math.PI));
drivetrain.setControl(drive.withRotationalRate(-force*2));
}
else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
drivetrain.setControl(drive.withRotationalRate(force*180/Math.PI));
drivetrain.setControl(drive.withRotationalRate(force*2));
}
}
}
// Called once the command ends or is interrupted.
@Override
@@ -67,6 +72,6 @@ public class TournerVersReservoir extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 10;
return drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 10;
}
}