From 36a6bf2ab07f9c5b4599c04906b867be2adb2ea6 Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Tue, 24 Mar 2026 19:39:38 -0400 Subject: [PATCH] grimper regle par side --- .../robot/commands/ModeAuto/GrimperMur.java | 28 +++++++++++++++---- .../commands/ModeAuto/GrimperReservoir.java | 28 +++++++++++++++---- 2 files changed, 46 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java index 3ebd3d5..b2a1686 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java +++ b/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; + Pigeon2 pigeon2; + Optional 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){ - drivetrain.setControl(drive.withVelocityX(2.961328-boty).withVelocityY(1.11-botx)); + if(angle-pigeon2.getYaw().getValueAsDouble() < angle+10 && angle- pigeon2.getYaw().getValueAsDouble() > angle-10){ + 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); } } diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java index ccd16ea..3f26544 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ b/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; + Pigeon2 pigeon2; + Optional 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){ - drivetrain.setControl(drive.withVelocityX(5.081328-boty).withVelocityY(1.11-botx)); + if(angle-pigeon2.getYaw().getValueAsDouble() < angle+10 && angle- pigeon2.getYaw().getValueAsDouble() > angle-10){ + 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); } }