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

oui

Browse Source
This commit is contained in:
samuel desharnais
2026-03-28 16:01:40 -04:00
parent 799e3e34c3
commit 755d79aa18
5 changed files with 16 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -42,19 +42,21 @@ public class TournerVersReservoir extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(alliance.get() == Alliance.Blue){
if(alliance.isPresent()){
if(alliance.get() == Alliance.Blue){
force = 0.5;
angle = 180;
}
else{
force = -0.5;
angle = 0;
}
}
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){
drivetrain.setControl(drive.withRotationalRate(-force));
drivetrain.setControl(drive.withRotationalRate(-force*180/Math.PI));
}
else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
drivetrain.setControl(drive.withRotationalRate(force));
drivetrain.setControl(drive.withRotationalRate(force*180/Math.PI));
}
}
@@ -65,6 +67,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 + 5;
return drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 10;
}
}