grimper regle par side

src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java

@@ -8,10 +8,14 @@ import static edu.wpi.first.units.Units.MetersPerSecond;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 
+import java.util.Optional;
+
 import com.ctre.phoenix6.hardware.Pigeon2;
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
@@ -24,9 +28,13 @@ public class GrimperMur extends Command {
   double[] BotPose = new double[6];
   double botx;
   double boty;
+  double x;
+  double y;
+  double angle;
   private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
   private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
   Pigeon2 pigeon2;
+  Optional<Alliance> alliance = DriverStation.getAlliance();
   private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
             .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
             .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
@@ -45,14 +53,24 @@ public class GrimperMur extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    if(alliance.get() == Alliance.Blue){
+     y = 2.961328;
+     x = 1.11;
+     angle = 180;
+    }
+    else{
+     x = 11.45966;
+     y = 6.959326;
+     angle = 0;
+    }
     BotPose = limeLight3.getBotPoseBlue();
     botx = BotPose[0];
     boty = BotPose[1];
-    if(180-pigeon2.getYaw().getValueAsDouble() < 10 && 180- pigeon2.getYaw().getValueAsDouble() > -10){
+    if(angle-pigeon2.getYaw().getValueAsDouble() < angle+10 && angle- pigeon2.getYaw().getValueAsDouble() > angle-10){
-      drivetrain.setControl(drive.withVelocityX(2.961328-boty).withVelocityY(1.11-botx));
+      drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx));
     }
     else{
-       drivetrain.setControl(drive.withRotationalRate(180-pigeon2.getYaw().getValueAsDouble()/45));
+       drivetrain.setControl(drive.withRotationalRate(angle-pigeon2.getYaw().getValueAsDouble()*Math.PI/180));
     }
   }
 
@@ -65,6 +83,6 @@ public class GrimperMur extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return (2.961328-boty < 0.05 || 2.961328-boty >-0.05) && (1.11-botx < 0.05 || 1.11-botx > -0.05);
+    return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05);
   }
 }

src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java

@@ -8,10 +8,14 @@ import static edu.wpi.first.units.Units.MetersPerSecond;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 
+import java.util.Optional;
+
 import com.ctre.phoenix6.hardware.Pigeon2;
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
@@ -24,9 +28,13 @@ public class GrimperReservoir extends Command {
   double[] BotPose = new double[6];
   double botx;
   double boty;
+  double x;
+  double y;
+  double angle;
   private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
   private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
   Pigeon2 pigeon2;
+    Optional<Alliance> alliance = DriverStation.getAlliance(); 
   private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
             .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
             .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
@@ -45,14 +53,24 @@ public class GrimperReservoir extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    if(alliance.get() == Alliance.Blue){
+     y = 5.081328;
+     x = 1.11;
+     angle = 0;
+    }
+    else{
+     y = 6.959326;
+     x = 13.57966;
+     angle = 180;
+    }
     BotPose = limeLight3G.getBotPoseBlue();
     botx = BotPose[0];
     boty = BotPose[1];
-    if(0-pigeon2.getYaw().getValueAsDouble() < 10 && 0- pigeon2.getYaw().getValueAsDouble() > -10){
+    if(angle-pigeon2.getYaw().getValueAsDouble() < angle+10 && angle- pigeon2.getYaw().getValueAsDouble() > angle-10){
-      drivetrain.setControl(drive.withVelocityX(5.081328-boty).withVelocityY(1.11-botx));
+      drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx));
     }
     else{
-       drivetrain.setControl(drive.withRotationalRate(0-pigeon2.getYaw().getValueAsDouble()/45));
+       drivetrain.setControl(drive.withRotationalRate(angle-pigeon2.getYaw().getValueAsDouble()*Math.PI/180));
     }
   }
 
@@ -65,6 +83,6 @@ public class GrimperReservoir extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return 5.081328-boty < 0.05 || 5.081328-boty >-0.05 && 1.11-botx < 0.05 || 1.11-botx > -0.05;
+    return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05);
   }
 }