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

debug de samedi

Browse Source
This commit is contained in:
samuel desharnais
2026-03-21 16:11:24 -04:00
parent e0c1e26933
commit a7fb3108ea
18 changed files with 178 additions and 150 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
src/main/java/frc/robot/commands/Limelighter.java
View File

@@ -21,41 +21,60 @@ public class Limelighter extends Command {
CommandSwerveDrivetrain drivetrain;
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
double botx;
double boty;
double angle;
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
.withDriveRequestType(DriveRequestType.OpenLoopVoltage);
double botx;
double boty;
double angle;
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
.withDriveRequestType(DriveRequestType.OpenLoopVoltage);
/** Creates a new Limelighter. */
public Limelighter(Limelight3G limelight3g,CommandSwerveDrivetrain drivetrain) {
public Limelighter(Limelight3G limelight3g, CommandSwerveDrivetrain drivetrain) {
this.limelight3g = limelight3g;
this.drivetrain = drivetrain;
addRequirements(drivetrain,limelight3g);
addRequirements(drivetrain, limelight3g);
timer = new Timer();
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
public void initialize() {
timer.reset();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double[] BotPose = new double[6];
BotPose = limelight3g.getBotPoseBlue();
botx = BotPose[0];
boty = BotPose[1];
angle = BotPose[5];
drivetrain.setControl(drive.withRotationalRate(limelight3g.Calcule(4.625594, botx, 4.034536, boty, BotPose[5])/90));
if(limelight3g.Calcule(4.625594, botx, 4.034536, boty, angle)/90 < 0.1){
timer.start();
if (limelight3g.getV()) {
BotPose = limelight3g.getBotPoseBlue();
botx = BotPose[1];
boty = BotPose[0];
angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
if(angle >180){
angle =- 360;
}
double calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 20;
drivetrain.setControl(
drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
System.out.println(calcul);
if (calcul < 0.2 && calcul > -0.2) {
// timer.start();
// } else {
// timer.reset();
// }
// } else {
// timer.stop();
drivetrain.setControl(drive.withRotationalRate(0));
}
}
else{
timer.reset();
drivetrain.setControl(drive.withRotationalRate(0));
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
@@ -66,6 +85,6 @@ public class Limelighter extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return timer.get() > 1;
return false;
}
}