diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0295b88..57b9cea 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -42,14 +42,14 @@ public class RobotContainer { NamedCommands.registerCommand("L3", new L3(elevateur, pince)); NamedCommands.registerCommand("CoralExpire",new CoralExpire(pince,bougie)); NamedCommands.registerCommand("CoralInspire", new CoralInspire(pince,bougie)); - NamedCommands.registerCommand("CoraletAlgue", new CoralAlgueInspire(pince)); + NamedCommands.registerCommand("CoraletAlgue", new CoralAlgueInspire(pince,bougie)); } private void configureBindings() { // manette1 - manette1.a().whileTrue(new AlgueExpire(pince)); - manette1.b().whileTrue(new CoralAlgueInspire(pince)); + manette1.a().whileTrue(new AlgueExpire(pince,bougie)); + manette1.b().whileTrue(new CoralAlgueInspire(pince,bougie)); manette1.x().whileTrue(new CoralInspire(pince,bougie)); manette1.y().whileTrue(new CoralExpire(pince,bougie)); manette1.povUp().whileTrue(new L4(elevateur, pince)); diff --git a/src/main/java/frc/robot/commands/AlgueExpire.java b/src/main/java/frc/robot/commands/AlgueExpire.java index 5b34608..077fa7d 100644 --- a/src/main/java/frc/robot/commands/AlgueExpire.java +++ b/src/main/java/frc/robot/commands/AlgueExpire.java @@ -5,15 +5,18 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Bougie; import frc.robot.subsystems.Pince; /* 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 AlgueExpire extends Command { private Pince pince; + private Bougie bougie; /** Creates a new CoralAlgue. */ - public AlgueExpire(Pince pince) { + public AlgueExpire(Pince pince,Bougie bougie) { this.pince = pince; - addRequirements(pince); + this.bougie = bougie; + addRequirements(pince,bougie); // Use addRequirements() here to declare subsystem dependencies. } @@ -24,7 +27,14 @@ public class AlgueExpire extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - pince.aspirealgue(0.5); + if(pince.emperagealgue()>8){ + pince.aspirealgue(0.5); + } + else{ + pince.aspirealgue(0.5); + bougie.Jaune(); + } + } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/CoralAlgueInspire.java b/src/main/java/frc/robot/commands/CoralAlgueInspire.java index 7382f67..4697090 100644 --- a/src/main/java/frc/robot/commands/CoralAlgueInspire.java +++ b/src/main/java/frc/robot/commands/CoralAlgueInspire.java @@ -5,15 +5,18 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Bougie; import frc.robot.subsystems.Pince; /* 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 CoralAlgueInspire extends Command { private Pince pince; + private Bougie bougie; /** Creates a new CoralAlgue. */ - public CoralAlgueInspire(Pince pince) { + public CoralAlgueInspire(Pince pince, Bougie bougie) { this.pince = pince; - addRequirements(pince); + this.bougie = bougie; + addRequirements(pince,bougie); // Use addRequirements() here to declare subsystem dependencies. } @@ -27,6 +30,7 @@ public class CoralAlgueInspire extends Command { pince.aspirecoral(-.5); if(pince.emperagealgue()>8){ pince.aspirealgue(0); + bougie.Bleu(); } else{ pince.aspirealgue(0.5); diff --git a/src/main/java/frc/robot/commands/CoralExpire.java b/src/main/java/frc/robot/commands/CoralExpire.java index ebb90db..c2932e5 100644 --- a/src/main/java/frc/robot/commands/CoralExpire.java +++ b/src/main/java/frc/robot/commands/CoralExpire.java @@ -31,7 +31,7 @@ public class CoralExpire extends Command { } else{ pince.aspirecoral(-.5); - bougie.Rouge();} + bougie.Jaune();} } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/CoralInspire.java b/src/main/java/frc/robot/commands/CoralInspire.java index 2ec67f3..fc249ba 100644 --- a/src/main/java/frc/robot/commands/CoralInspire.java +++ b/src/main/java/frc/robot/commands/CoralInspire.java @@ -27,7 +27,6 @@ public class CoralInspire extends Command { @Override public void execute() { if(pince.emperagecoral()>8){ - bougie.Vert(); pince.aspirecoral(0); } else{ diff --git a/src/main/java/frc/robot/commands/L2.java b/src/main/java/frc/robot/commands/L2.java index 179742a..ca5b8ec 100644 --- a/src/main/java/frc/robot/commands/L2.java +++ b/src/main/java/frc/robot/commands/L2.java @@ -5,6 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Bougie; import frc.robot.subsystems.Elevateur; import frc.robot.subsystems.Pince; @@ -38,6 +39,7 @@ public class L2 extends Command { } if(pince.encodeurpivot()>=500 && pince.encodeurpivot()<=510){ pince.pivote(0); + } else if(pince.encodeurpivot()>=510){ pince.pivote(-0.3); @@ -45,6 +47,7 @@ public class L2 extends Command { else{ pince.pivote(0.3); } + } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/L3.java b/src/main/java/frc/robot/commands/L3.java index 273a609..c933027 100644 --- a/src/main/java/frc/robot/commands/L3.java +++ b/src/main/java/frc/robot/commands/L3.java @@ -5,6 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Bougie; import frc.robot.subsystems.Elevateur; import frc.robot.subsystems.Pince; @@ -15,7 +16,7 @@ public class L3 extends Command { /** Creates a new L2. */ public L3(Elevateur elevateur,Pince pince) { this.elevateur = elevateur; - this.pince = pince; + this.pince = pince; addRequirements(elevateur,pince); // Use addRequirements() here to declare subsystem dependencies. } diff --git a/src/main/java/frc/robot/commands/L4.java b/src/main/java/frc/robot/commands/L4.java index b55fc3d..9cbcaf0 100644 --- a/src/main/java/frc/robot/commands/L4.java +++ b/src/main/java/frc/robot/commands/L4.java @@ -5,6 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Bougie; import frc.robot.subsystems.Elevateur; import frc.robot.subsystems.Pince; diff --git a/src/main/java/frc/robot/subsystems/Bougie.java b/src/main/java/frc/robot/subsystems/Bougie.java index 5dfcf91..98bdf49 100644 --- a/src/main/java/frc/robot/subsystems/Bougie.java +++ b/src/main/java/frc/robot/subsystems/Bougie.java @@ -20,14 +20,29 @@ public class Bougie extends SubsystemBase { candle.configAllSettings(config); } public void Rouge() { - candle.setLEDs(255, 0, 0); + candle.setLEDs(255, 0, 0,0,8,8); + candle.setLEDs(255, 0, 0,0,24,8); + candle.setLEDs(255, 0, 0,0,40,8); + candle.setLEDs(255, 0, 0,0,56,8); } public void Vert() { - candle.setLEDs(0, 255, 0); + 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); } public void Bleu() { - candle.setLEDs(0, 0, 255); + candle.setLEDs(0, 0, 255,0,16,8); + candle.setLEDs(0, 0, 255,0,32,8); + candle.setLEDs(0, 0, 255,0,48,8); + candle.setLEDs(0, 0, 255,0,64,8); } + public void Jaune() { + candle.setLEDs(255, 215, 0,0,16,8); + candle.setLEDs(255, 215, 0,0,32,8); + candle.setLEDs(255, 215, 0,0,48,8); + candle.setLEDs(255, 215, 0,0,64,8); + } public void RainBow(){candle.animate(rainbowAnim);} public void RainBowStop(){candle.animate(null);} @Override