led fini

src/main/java/frc/robot/RobotContainer.java

@@ -17,6 +17,7 @@ import frc.robot.commands.MonterGrimpeur;
 import frc.robot.subsystems.Balayeuse;
 import frc.robot.subsystems.Grimpeur;
 import frc.robot.subsystems.Lanceur;
+import frc.robot.subsystems.Led;
 import frc.robot.subsystems.LimeLight3;
 
 public class RobotContainer {
@@ -24,6 +25,7 @@ public class RobotContainer {
   Grimpeur grimpeur = new Grimpeur();
   Lanceur lanceur = new Lanceur();
   LimeLight3 limeLight3 = new LimeLight3();
+  Led led = new Led();
   CommandXboxController manette = new CommandXboxController(0);
   public RobotContainer() {
     CameraServer.startAutomaticCapture();
@@ -31,12 +33,12 @@ public class RobotContainer {
   }
 
   private void configureBindings() {
-    manette.a().whileTrue(new Lancer(lanceur,limeLight3));
+    manette.a().whileTrue(new Lancer(lanceur,limeLight3,balayeuse,led));
     manette.rightTrigger().whileTrue(new MonterGrimpeur(grimpeur));
     manette.leftTrigger().whileTrue(new DescendreGrimpeur(grimpeur));
     manette.rightBumper().whileTrue(new MonterBalyeuse(balayeuse));
     manette.leftBumper().whileTrue(new DescendreBalyeuse(balayeuse));
-    manette.b().whileTrue(new Aspirer(balayeuse));
+    manette.b().whileTrue(new Aspirer(balayeuse,led));
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/commands/Aspirer.java

@@ -4,33 +4,56 @@
 
 package frc.robot.commands;
 
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Balayeuse;
+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;
   /** Creates a new Aspirer. */
-  public Aspirer(Balayeuse balayeuse) {
+  public Aspirer(Balayeuse balayeuse, Led led) {
     this.balayeuse = balayeuse;
-    addRequirements(balayeuse); 
+    this.led = led;
+    this.timer = new Timer();
+    addRequirements(balayeuse, led); 
     // 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() {
-    balayeuse.Balayer(0.5);
+    timer.start();
+    if(balayeuse.Amp() < 40){
+      timer.reset();
+      balayeuse.Balayer(0.5);
+    }
+    else if(balayeuse.Amp() > 40){
+      if(timer.get() > 2){
+        balayeuse.Balayer(0);
+        led.Jaune2();
+      }
+      else{
+        balayeuse.Balayer(0.5);
+      }
+    }
+
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
     balayeuse.Balayer(0);
+    timer.stop();
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/commands/Lancer.java

@@ -5,8 +5,11 @@
 package frc.robot.commands;
 
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.Timer;
 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;
 
@@ -15,10 +18,15 @@ public class Lancer extends Command {
   private Lanceur lanceur;
   private PIDController pidController;
   private Limelight3G limeLight3G;
-  private double output;
+  private Timer timer;
+  private Balayeuse balayeuse;
+  private Led led;
   /** Creates a new Lancer. */
-  public Lancer(Lanceur lanceur, LimeLight3 limeLight3) {
+  public Lancer(Lanceur lanceur, LimeLight3 limeLight3, Balayeuse balayeuse,Led led) {
     this.lanceur = lanceur;
+    this.balayeuse = balayeuse;
+    this.led = led;
+    this.timer = new Timer();
     this.limeLight3G = new Limelight3G();
     addRequirements(lanceur);
     // Use addRequirements() here to declare subsystem dependencies.
@@ -28,17 +36,41 @@ public class Lancer extends Command {
   @Override
   public void initialize() {
     pidController = new PIDController(0, 0,0, 0);
+    timer.reset();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double vitesse = (100-limeLight3G.getTA())/100;
+    timer.start();
-    double output = pidController.calculate(lanceur.Vitesse(),vitesse);
+    if(lanceur.Amp() > 40){ 
-    lanceur.Lancer(output);
+      timer.reset();
-    if(lanceur.Vitesse() >= vitesse){
+      double vitesse = (100-limeLight3G.getTA())/100;
-      lanceur.Demeler(0.5);
+      double output = pidController.calculate(lanceur.Vitesse(),vitesse);
+      lanceur.Lancer(output);
+      if(lanceur.Vitesse() >= vitesse){
+        lanceur.Demeler(0.5);
+      }
     }
+    else if(lanceur.Amp() < 40){
+      lanceur.Lancer(0);
+      lanceur.Demeler(0);
+      if(!balayeuse.GetLimiSwtich()){
+        balayeuse.Pivoter(0.2);
+      }
+      else{
+        balayeuse.Reset();
+        balayeuse.Pivoter(0);
+        double vitesse = (100-limeLight3G.getTA())/100;
+        double output = pidController.calculate(lanceur.Vitesse(),vitesse);
+        lanceur.Lancer(output);
+        if(lanceur.Vitesse() >= vitesse){
+          lanceur.Demeler(0.5);
+        }
+      }
+      led.Vert2();
+    }
+
   }
 
   // Called once the command ends or is interrupted.
@@ -46,6 +78,7 @@ public class Lancer extends Command {
   public void end(boolean interrupted) {
     lanceur.Demeler(0);
     lanceur.Lancer(0);
+    balayeuse.Pivoter(0);
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/subsystems/Balayeuse.java

@@ -8,7 +8,9 @@ import com.revrobotics.spark.SparkFlex;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
+import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Balayeuse extends SubsystemBase {
@@ -32,6 +34,14 @@ public class Balayeuse extends SubsystemBase {
   public boolean GetLimiSwtich(){
     return limit.get();
   }
+  public double Amp(){
+    return Balaye2.getOutputCurrent();
+  }
+  public void Temps(){
+    Timer timer = new Timer();
+    timer.start();
+    
+  }
   /** Creates a new Balayeuse. */
   public Balayeuse() {}
 

src/main/java/frc/robot/subsystems/Lanceur.java

@@ -23,6 +23,9 @@ public class Lanceur extends SubsystemBase {
     public double Vitesse(){
       return moteur1.getEncoder().getVelocity();
     }
+    public double Amp(){
+      return moteur1.getOutputCurrent();
+    }
   /** Creates a new Lanceur. */
   public Lanceur() {
   }

src/main/java/frc/robot/subsystems/Led.java

@@ -16,13 +16,60 @@ public class Led extends SubsystemBase {
   CANdle CANDle = new CANdle(7);
   RainbowAnimation rainbowAnim = new RainbowAnimation();
     public void bleu(){
-    CANDle.setLEDs(0, 0, 255);
+    CANDle.setLEDs(0, 0, 255,0,0,8);
+    CANDle.setLEDs(0, 0, 255,0,16,8);
+    CANDle.setLEDs(0, 0, 255,0,32,8);
+    CANDle.setLEDs(0, 0, 255,0,56,8);
+    CANDle.setLEDs(0, 0, 255,0,72,8);
+    CANDle.setLEDs(0, 0, 255,0,88,8);
+    CANDle.setLEDs(0, 0, 255,0,104,8);
+    CANDle.setLEDs(0, 0, 255,0,120,8);
+    CANDle.setLEDs(0, 0, 255,0,136,8);
+    
   }
-  public void Vert(){
+  public void Vert1(){
-    CANDle.setLEDs(0,255, 0);
+    CANDle.setLEDs(0, 255, 0,0,0,8);
+    CANDle.setLEDs(0, 255, 0,0,16,8);
+    CANDle.setLEDs(0, 255, 0,0,32,8);
+    CANDle.setLEDs(0, 255, 0,0,56,8);
+    CANDle.setLEDs(0, 255, 0,0,72,8);
+    CANDle.setLEDs(0, 255, 0,0,88,8);
+    CANDle.setLEDs(0, 255, 0,0,104,8);
+    CANDle.setLEDs(0, 255, 0,0,120,8);
+    CANDle.setLEDs(0, 255, 0,0,136,8);
   }
   public void Rouge(){
-    CANDle.setLEDs(255,0, 0);
+    CANDle.setLEDs(255, 0, 0,0,0,8);
+    CANDle.setLEDs(255, 0, 0,0,16,8);
+    CANDle.setLEDs(255, 0, 0,0,32,8);
+    CANDle.setLEDs(255, 0, 0,0,48,8);
+    CANDle.setLEDs(255, 0, 0,0,64,8);
+    CANDle.setLEDs(255, 0, 0,0,80,8);
+    CANDle.setLEDs(255, 0, 0,0,96,8);
+    CANDle.setLEDs(255, 0, 0,0,112,8);
+    CANDle.setLEDs(255, 0, 0,0,128,8);
+  }
+  public void Jaune2(){
+    CANDle.setLEDs(255, 255, 0,0,8,8);
+    CANDle.setLEDs(255, 255, 0,0,24,8);
+    CANDle.setLEDs(255, 255, 0,0,40,8);
+    CANDle.setLEDs(255, 255, 0,0,56,8);
+    CANDle.setLEDs(255, 255, 0,0,72,8);
+    CANDle.setLEDs(255, 255, 0,0,88,8);
+    CANDle.setLEDs(255, 255, 0,0,104,8);
+    CANDle.setLEDs(255, 255, 0,0,120,8);
+    CANDle.setLEDs(255, 255, 0,0,136,8);
+  }
+  public void Vert2(){
+    CANDle.setLEDs(0, 255, 0,0,8,8);
+    CANDle.setLEDs(0, 255, 0,0,24,8);
+    CANDle.setLEDs(0, 255, 0,0,40,8);
+    CANDle.setLEDs(0, 255, 0,0,56,8);
+    CANDle.setLEDs(0, 255, 0,0,72,8);
+    CANDle.setLEDs(0, 255, 0,0,88,8);
+    CANDle.setLEDs(0, 255, 0,0,104,8);
+    CANDle.setLEDs(0, 255, 0,0,120,8);
+    CANDle.setLEDs(0, 255, 0,0,136,8);
   }
   public void RainBow(){
     CANDle.animate(rainbowAnim);
@@ -46,7 +93,7 @@ public class Led extends SubsystemBase {
   public void periodic() {
     double temps = DriverStation.getMatchTime();
     if(temps > 20 && temps < 30){
-      Vert();
+      Vert1();
     }
     if(Equipe()){
       if(temps > 30 && temps < 55){