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

debug samedi

Browse Source
This commit is contained in:
samuel desharnais
2026-03-28 13:20:52 -04:00
parent 79568a58b9
commit ba57959f4d
8 changed files with 332 additions and 304 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
src/main/java/frc/robot/commands/Lancer.java
View File

@@ -4,7 +4,11 @@
package frc.robot.commands;
import java.util.Optional;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Lanceur;
@@ -16,6 +20,11 @@ public class Lancer extends Command {
private PIDController pidController;
private Limelight3G limeLight3G;
private Timer timer;
double vitesse = 0.5;
double botx = 0;
double boty = 0;
Optional<Alliance> alliance = DriverStation.getAlliance();
/** Creates a new Lancer. */
public Lancer(Lanceur lanceur, Limelight3G limeLight3G) {
this.lanceur = lanceur;
@@ -28,6 +37,7 @@ public class Lancer extends Command {
// Called when the command is initially scheduled.
@Override
public void initialize() {
if(limeLight3G.getV()){vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250;}
pidController = new PIDController(0.0007, 0,0, 0.001);
timer.reset();
}
@@ -35,17 +45,19 @@ public class Lancer extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double botx = 0;
double boty = 0;
double[] BotPose = new double[6];
if(limeLight3G.getV()){
BotPose = limeLight3G.getBotPoseBlue();
if(alliance.get() == Alliance.Blue){
BotPose = limeLight3G.getBotPoseBlue();
}
else{
BotPose = limeLight3G.getBotPoseRed();
}
botx = BotPose[0];
boty = BotPose[1];
}
double vitesse = 0.5;
}
if(limeLight3G.getV()){
vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250;
System.out.println(vitesse);
double output = pidController.calculate(lanceur.Vitesse(),vitesse);