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:
Antoine PerreaultE
2026-04-01 19:27:12 -04:00
parent 56b28100df
commit b0ad34519e
5 changed files with 49 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java
View File

@@ -53,7 +53,7 @@ public class LimelighterAuto extends Command {
double[] BotPose = new double[6];
System.out.println("e");
if (limelight3g.getV()) {
// BotPose = limelight3g.getBotPoseBlue();
BotPose = limelight3g.getBotPoseBlue();
if (!alliance.isPresent()) {
return;
}
@@ -65,25 +65,25 @@ public class LimelighterAuto extends Command {
x = 11.915394;
BotPose = limelight3g.getBotPoseRed();
}
botx = BotPose[1];
boty = BotPose[0];
angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
botx = BotPose[0];
boty = BotPose[1];
angle = BotPose[5];
calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
if(angle > 180){
angle -= 360;
}
if(calcul > -5 && calcul < 5){
drivetrain.setControl(
drive.withRotationalRate(0));
}
else if(calcul > 5){
drivetrain.setControl(
drive.withRotationalRate(0.5*(2*Math.PI)));
drive.withRotationalRate(2));
}
else if(calcul < -5){
drivetrain.setControl(
drive.withRotationalRate(-0.5*(2*Math.PI)));
drivetrain.setControl(drive.withRotationalRate(-2));
}
// botx = BotPose[1];
// boty = BotPose[0];
// angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
// calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
// if(calcul < -5 && calcul > -180){
// drivetrain.setControl(
// drive.withRotationalRate(0.5*(2*Math.PI)));
@@ -92,7 +92,7 @@ public class LimelighterAuto extends Command {
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }
// else if(calcul >= 180){
// else if(calcul < -5){
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }
@@ -126,6 +126,7 @@ public class LimelighterAuto extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return calcul > -5 && calcul < 5;
return calcul > -5 && calcul < 5;
}
}