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

grimpe en rouge aussi

Browse Source
This commit is contained in:
Antoine PerreaultE
2026-03-31 19:59:15 -04:00
parent 42e35fca8a
commit 7b535a845f
1 changed files with 15 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java
View File

@@ -63,8 +63,6 @@ public class GrimperReservoir extends Command {
BotPose = limeLight3G.getBotPoseBlue(); BotPose = limeLight3G.getBotPoseBlue();
botx = BotPose[0]; botx = BotPose[0];
boty = BotPose[1]; boty = BotPose[1];
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180;
System.out.println(pigeonAngle);
if(angle < 0){ if(angle < 0){
angle = angle + 360; angle = angle + 360;
} }
@@ -85,23 +83,25 @@ public class GrimperReservoir extends Command {
} }
} }
else{ else{
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180;
System.out.println(pigeonAngle);
y = 2.6; y = 2.6;
x = 1.11; x = 1.11;
angle = 0; angle = 0;
if(pigeonAngle> 358 || pigeonAngle< 2){ if(pigeonAngle> 358 || pigeonAngle< 2){
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)).withRotationalRate(0)); drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)).withRotationalRate(0));
}
else{
if(pigeonAngle>0 && pigeonAngle<180){
drivetrain.setControl(drive.withRotationalRate(-1));
System.out.println("x");
} }
else if(pigeonAngle>180){ else{
System.out.println("e"); if(pigeonAngle>0 && pigeonAngle<180){
drivetrain.setControl(drive.withRotationalRate(1)); drivetrain.setControl(drive.withRotationalRate(-1));
System.out.println("x");
}
else if(pigeonAngle>180){
System.out.println("e");
drivetrain.setControl(drive.withRotationalRate(1));
}
} }
} }
}
} }
else{ else{
drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0)); drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0));