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

mode auto pls marche pls

Browse Source
This commit is contained in:
Antoine PerreaultE
2026-03-26 17:46:22 -04:00
parent 69fe4a58d0
commit d0c50cbd6e
8 changed files with 272 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -28,6 +28,7 @@ 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;
@@ -50,36 +51,86 @@ public class RetourMilieuDroite extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
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;
}
else{
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)){
drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0));
}
else{
if(botx < 6){
drivetrain.setControl(drive.withVelocityX(y-boty));
}
else{
drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx));
}
}
}
else{
if(pigeon2.getYaw().getValueAsDouble() >angle && pigeon2.getYaw().getValueAsDouble() <angle +180){
drivetrain.setControl(drive.withRotationalRate(0.5));
}
else if(pigeon2.getYaw().getValueAsDouble() >=angle +180){
drivetrain.setControl(drive.withRotationalRate(-0.5));
}
}
}
}
else{
y = 7.380;
x = 13.963;
angle = 180;
}
if(pigeon2.getYaw().getValueAsDouble() <355 || pigeon2.getYaw().getValueAsDouble() > 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));
}
}
else{
}
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));
}
}
}
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
public void end(boolean interrupted) {
drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0));
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05);
}
}