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

40
src/main/java/frc/robot/commands/Aspirer.java
View File

@@ -12,58 +12,30 @@ import frc.robot.subsystems.Led;
/* 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 */
public class Aspirer extends Command {
private Balayeuse balayeuse;
private Timer timer;
private Led led;
private double temp;
/** Creates a new Aspirer. */
public Aspirer(Balayeuse balayeuse, Led led) {
public Aspirer(Balayeuse balayeuse) {
this.balayeuse = balayeuse;
this.led = led;
this.timer = new Timer();
addRequirements(balayeuse, led);
this.temp = 0;
addRequirements(balayeuse);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
timer.reset();
temp = balayeuse.Amp();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
timer.start();
int nbFois = 0;
double moyenneAmp = 0;
if(timer.get() <3){
nbFois++;
moyenneAmp += balayeuse.Amp() / nbFois;
}
else{
nbFois++;
moyenneAmp -= temp;
moyenneAmp += balayeuse.Amp() / nbFois;
temp = balayeuse.Amp();
}
if(moyenneAmp < balayeuse.AmpMax()){
timer.reset();
balayeuse.Balayer(-0.5);
}
else{
balayeuse.Balayer(0);
led.Jaune2();
}
balayeuse.BalayerEnbas(-0.5);
balayeuse.BalayerPadle(-0.2);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
balayeuse.Balayer(0);
timer.stop();
balayeuse.BalayerEnbas(0);
balayeuse.BalayerPadle(0);
}
// Returns true when the command should end.

2
src/main/java/frc/robot/commands/DescendreBalyeuse.java
View File

@@ -25,7 +25,7 @@ public class DescendreBalyeuse extends Command {
@Override
public void execute() {
if(Math.abs(balayeuse.Distance()) < balayeuse.EncodeurBalayeuse()){
balayeuse.Pivoter(-0.2);
balayeuse.Pivoter(-0.5);
}
else{
balayeuse.Pivoter(0);

2
src/main/java/frc/robot/commands/DescendreGrimpeur.java
View File

@@ -24,7 +24,7 @@ public class DescendreGrimpeur extends Command {
@Override
public void execute() {
if(!grimpeur.Limit()){
grimpeur.Grimper(-0.3);
grimpeur.Grimper(-0.4);
}
else{
grimpeur.Reset();

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

@@ -18,17 +18,13 @@ public class Lancer extends Command {
private PIDController pidController;
private Limelight3G limeLight3G;
private Timer timer;
private Balayeuse balayeuse;
private Led led;
private double temp;
/** Creates a new Lancer. */
public Lancer(Lanceur lanceur, Limelight3G limeLight3G, Balayeuse balayeuse,Led led) {
public Lancer(Lanceur lanceur, Limelight3G limeLight3G) {
this.lanceur = lanceur;
this.balayeuse = balayeuse;
this.led = led;
this.timer = new Timer();
this.limeLight3G = new Limelight3G();
addRequirements(lanceur, balayeuse, led, limeLight3G);
addRequirements(lanceur, limeLight3G);
this.temp = 0;
// Use addRequirements() here to declare subsystem dependencies.
}
@@ -36,9 +32,8 @@ public class Lancer extends Command {
// Called when the command is initially scheduled.
@Override
public void initialize() {
pidController = new PIDController(1, 0,0, 0);
pidController = new PIDController(0.0007, 0,0, 0.001);
timer.reset();
timer.start();
temp = lanceur.Amp();
}
@@ -52,40 +47,22 @@ public class Lancer extends Command {
BotPose = limeLight3G.getBotPoseBlue();
botx = BotPose[0];
boty = BotPose[1];
}
int nbFois = 0;
double moyenneAmp = 0;
if(timer.get() < 3){
nbFois++;
moyenneAmp += balayeuse.Amp() / nbFois;
}
else{
nbFois++;
moyenneAmp -= temp;
moyenneAmp += balayeuse.Amp() / nbFois;
temp = balayeuse.Amp();
}
// if(moyenneAmp > lanceur.AmpBas() && nbFois > 10){
timer.reset();
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.1);
}
// }
vitesse = 3100 + 475 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2))) - 2);
System.out.println(vitesse);
double output = pidController.calculate(lanceur.Vitesse(),vitesse);
lanceur.Lancer(output);
if(lanceur.Vitesse() >= vitesse-800){
timer.start();
if(timer.get() >0.5){
lanceur.Demeler(1);
}
}
}
}
// Called once the command ends or is interrupted.
@@ -93,7 +70,8 @@ public class Lancer extends Command {
public void end(boolean interrupted) {
lanceur.Demeler(0);
lanceur.Lancer(0);
balayeuse.Pivoter(0);
timer.reset();
timer.stop();
}
// Returns true when the command should end.

4
src/main/java/frc/robot/commands/LancerAspirer.java
View File

@@ -17,8 +17,8 @@ import frc.robot.subsystems.Limelight3G;
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class LancerAspirer extends ParallelCommandGroup {
/** Creates a new LacerAspirer. */
public LancerAspirer(Lanceur lanceur, Balayeuse balayeuse, LimeLight3 limeLight3, Led led) {
addCommands(new LancerBaseVitesse(lanceur, limeLight3), new Aspirer(balayeuse, led));
public LancerAspirer(Lanceur lanceur, Balayeuse balayeuse, Limelight3G limeLight3G) {
addCommands(new Lancer(lanceur, limeLight3G), new Aspirer(balayeuse));
// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
}

16
src/main/java/frc/robot/commands/LancerBaseVitesse.java
View File

@@ -19,9 +19,9 @@ public class LancerBaseVitesse extends Command {
private Timer timer;
private double temp;
/** Creates a new Lancer. */
public LancerBaseVitesse(Lanceur lanceur, LimeLight3 limeLight3) {
public LancerBaseVitesse(Lanceur lanceur, Limelight3G limeLight3G) {
this.lanceur = lanceur;
// this.timer = new Timer();
this.timer = new Timer();
this.limeLight3G = new Limelight3G();
addRequirements(lanceur, limeLight3G);
//this.temp = 0;
@@ -32,8 +32,8 @@ public class LancerBaseVitesse extends Command {
@Override
public void initialize() {
pidController = new PIDController(0.0007, 0,0, 0.001);
//timer.reset();
//timer.start();
timer.reset();
timer.start();
//temp = lanceur.Amp();
}
@@ -62,9 +62,9 @@ public class LancerBaseVitesse extends Command {
double vitesse = lanceur.vitesseDemander();
double output = pidController.calculate(lanceur.Vitesse(),vitesse);
lanceur.Lancer(output);
if(lanceur.Vitesse() >= 1200){
lanceur.Demeler(0.5);
}
if(timer.get() >1){
lanceur.Demeler(1);
}
// }
}
@@ -74,6 +74,8 @@ public class LancerBaseVitesse extends Command {
public void end(boolean interrupted) {
lanceur.Demeler(0);
lanceur.Lancer(0);
timer.reset();
timer.stop();
}
// Returns true when the command should end.

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;
}
}

6
src/main/java/frc/robot/commands/ModeAuto/AspirerAuto.java
View File

@@ -24,13 +24,15 @@ public class AspirerAuto extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
balayeuse.Balayer(0.5);
balayeuse.BalayerEnbas(-0.5);
balayeuse.BalayerPadle(-0.2);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
balayeuse.Balayer(0);
balayeuse.BalayerEnbas(0);
balayeuse.BalayerPadle(0);
}
// Returns true when the command should end.

7
src/main/java/frc/robot/commands/ModeOposer.java
View File

@@ -5,17 +5,14 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Balayeuse;
import frc.robot.subsystems.Lanceur;
/* 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 */
public class ModeOposer extends Command {
private Lanceur lanceur;
private Balayeuse balayeuse;
/** Creates a new Lancer. */
public ModeOposer(Lanceur lanceur, Balayeuse balayeuse) {
public ModeOposer(Lanceur lanceur) {
this.lanceur = lanceur;
this.balayeuse = balayeuse;
addRequirements(lanceur ,balayeuse);
addRequirements(lanceur);
// Use addRequirements() here to declare subsystem dependencies.
}

44
src/main/java/frc/robot/commands/ModeOposerBalayeuse.java Normal file
View File

@@ -0,0 +1,44 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Balayeuse;
import frc.robot.subsystems.Lanceur;
/* 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 */
public class ModeOposerBalayeuse extends Command {
private Balayeuse balayeuse;
/** Creates a new Lancer. */
public ModeOposerBalayeuse(Balayeuse balayeuse) {
this.balayeuse = balayeuse;
addRequirements(balayeuse);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
balayeuse.BalayerEnbas(0.5);
balayeuse.BalayerPadle(0.2);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
balayeuse.BalayerEnbas(0);
balayeuse.BalayerPadle(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

2
src/main/java/frc/robot/commands/MonterBalyeuse.java
View File

@@ -25,7 +25,7 @@ public class MonterBalyeuse extends Command {
@Override
public void execute() {
if(!balayeuse.GetLimiSwtich()){
balayeuse.Pivoter(0.2);
balayeuse.Pivoter(0.5);
}
else{
balayeuse.Reset();

2
src/main/java/frc/robot/commands/MonterGrimpeur.java
View File

@@ -25,7 +25,7 @@ public class MonterGrimpeur extends Command {
public void execute() {
if(Math.abs(grimpeur.Position()) < grimpeur.PositionFinal()){
grimpeur.Grimper(0.3);
grimpeur.Grimper(0.5);
System.out.println("monte");
}
else {