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

Merge branch 'main' of https://git.demerso.net/sdesharnais/Rebuilt-2026

Browse Source
This commit is contained in:
Antoine PerreaultE
2026-03-26 17:49:01 -04:00
parent d0c50cbd6e 0492c9a2c6
commit 5ba5cce953
21 changed files with 806 additions and 145 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java
View File

@@ -28,7 +28,6 @@ public class RetourMilieuDroite extends Command {
double x;
double y;
double angle;
double force;
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
Pigeon2 pigeon2;
@@ -54,16 +53,10 @@ public class RetourMilieuDroite extends Command {
if(angle < 0){
angle = angle + 360;
}
double[] BotPose = new double[6];
BotPose = limelight3g.getBotPoseBlue();
botx = BotPose[0];
boty = BotPose[1];
if(alliance.get() == Alliance.Blue){
y = 0.639;
x = 2.305;
angle = 0;
force = 0.5;
if(limelight3g.getV()){
if(pigeon2.getYaw().getValueAsDouble() >355 || pigeon2.getYaw().getValueAsDouble() < 5){
if((y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05)){
@@ -93,34 +86,16 @@ public class RetourMilieuDroite extends Command {
y = 7.380;
x = 13.963;
angle = 180;
force = -0.5;
if(limelight3g.getV()){
if(pigeon2.getYaw().getValueAsDouble() >175 && pigeon2.getYaw().getValueAsDouble() < 185){
if((y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05)){
drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0));
}
else{
if(botx > 10){
drivetrain.setControl(drive.withVelocityX(y-boty));
}
else{
drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx));
}
}
}
else{
if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){
drivetrain.setControl(drive.withRotationalRate(0.5));
}
else if(pigeon2.getYaw().getValueAsDouble() >=180){
drivetrain.setControl(drive.withRotationalRate(-0.5));
}
}
}
}
}
if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){
drivetrain.setControl(drive.withRotationalRate(0.5));
}
else if(pigeon2.getYaw().getValueAsDouble() >180){
drivetrain.setControl(drive.withRotationalRate(-0.5));
}
}
}
// Called once the command ends or is interrupted.
@Override