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

48
src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java
View File

@@ -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);
@@ -48,12 +56,42 @@ public class GrimperMur extends Command {
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 < 0){
angle = angle + 360;
}
if(alliance.get() == Alliance.Blue){
y = 2.961328;
x = 1.11;
angle = 0;
if(pigeon2.getYaw().getValueAsDouble() > 355 || pigeon2.getYaw().getValueAsDouble() < 5){
drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx));
}
else{
drivetrain.setControl(drive.withRotationalRate(180-pigeon2.getYaw().getValueAsDouble()/45));
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{
x = 11.45966;
y = 6.959326;
angle = 180;
if(pigeon2.getYaw().getValueAsDouble() > 175 && pigeon2.getYaw().getValueAsDouble() < 185){
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.
@@ -65,6 +103,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);
}
}
}