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

22
src/main/java/frc/robot/commands/Limelighter.java
View File

@@ -56,7 +56,7 @@ public class Limelighter extends Command {
return;
}
if (alliance.get() == Alliance.Blue) {
x = 4.6;
x = 4;
}
else {
x = 11.915394;
@@ -64,30 +64,28 @@ public class Limelighter extends Command {
botx = BotPose[1];
boty = BotPose[0];
angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
if(calcul < -5 && calcul > -180){
calcul = limelight3g.Calcule(botx, x, boty, 4.6, angle);
if(calcul < -3 && calcul > -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*(2*Math.PI)));
drive.withRotationalRate(-1.5));
}
else if(calcul > 5 && calcul < 180){
else if(calcul > 3 && calcul < 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*(2*Math.PI)));
drive.withRotationalRate(-1.5));
}
else if(calcul >= 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*(2*Math.PI)));
drive.withRotationalRate(-1.5));
}
else if(calcul <= -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*(2*Math.PI)));
drive.withRotationalRate(1.5));
}
else{
drivetrain.setControl(
drive.withRotationalRate(0));
}
drivetrain.setControl(
drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
System.out.println(angle);
}
System.out.println(calcul);
if (calcul < 0.2 && calcul > -0.2) {
drivetrain.setControl(drive.withRotationalRate(0));
}