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

balayeuse fait

Browse Source
This commit is contained in:
samuel desharnais
2026-03-10 20:39:18 -04:00
parent d04817a197
commit a142290bd5
8 changed files with 52 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -10,7 +10,6 @@ import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Balayeuse;
import frc.robot.subsystems.Lanceur;
import frc.robot.subsystems.Led;
import frc.robot.subsystems.LimeLight3;
import frc.robot.subsystems.Limelight3G;
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
@@ -37,7 +36,7 @@ public class Lancer extends Command {
// Called when the command is initially scheduled.
@Override
public void initialize() {
pidController = new PIDController(0, 0,0, 0);
pidController = new PIDController(1, 0,0, 0);
timer.reset();
timer.start();
temp = lanceur.Amp();
@@ -46,10 +45,14 @@ 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];
BotPose = limeLight3G.getBotPoseBlue();
double botx = BotPose[0];
double boty = BotPose[1];
if(limeLight3G.getV()){
BotPose = limeLight3G.getBotPoseBlue();
botx = BotPose[0];
boty = BotPose[1];
}
int nbFois = 0;
double moyenneAmp = 0;
@@ -63,21 +66,25 @@ public class Lancer extends Command {
moyenneAmp += balayeuse.Amp() / nbFois;
temp = balayeuse.Amp();
}
if(moyenneAmp > lanceur.AmpBas() && nbFois > 10){
// if(moyenneAmp > lanceur.AmpBas() && nbFois > 10){
timer.reset();
balayeuse.Balayer(0.5);
led.Jaune2();
}
else{
//pythagore |
// \/
double vitesse = Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2));
double output = pidController.calculate(lanceur.Vitesse(),vitesse);
balayeuse.Balayer(-0.5);
// led.Jaune2();
// }
// else{
double vitesse = 0.5;
if(limeLight3G.getV()){
//pythagore |
// \/
vitesse = Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2));
}
double output = vitesse; /*pidController.calculate(lanceur.Vitesse(),vitesse);*/
lanceur.Lancer(output);
if(lanceur.Vitesse() >= vitesse){
lanceur.Demeler(0.5);
}
}
// }
}