diff --git a/simgui-ds.json b/simgui-ds.json index c4b7efd..5628a9e 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/deploy/pathplanner/autos/DepotTirer.auto b/src/main/deploy/pathplanner/autos/DepotTirer.auto index b6fd578..7979d5a 100644 --- a/src/main/deploy/pathplanner/autos/DepotTirer.auto +++ b/src/main/deploy/pathplanner/autos/DepotTirer.auto @@ -35,12 +35,6 @@ "pathName": "Tir" } }, - { - "type": "named", - "data": { - "name": "Limelighter" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/Depot.path b/src/main/deploy/pathplanner/paths/Depot.path index 664610b..84a0f32 100644 --- a/src/main/deploy/pathplanner/paths/Depot.path +++ b/src/main/deploy/pathplanner/paths/Depot.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 0.4938659058487873, + "x": 0.23509272467902986, "y": 5.917574893009986 }, "prevControl": { - "x": 1.0372895863052785, + "x": 0.778516405135521, "y": 5.930513552068473 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/GrimperMur.path b/src/main/deploy/pathplanner/paths/GrimperMur.path index 0907170..890c517 100644 --- a/src/main/deploy/pathplanner/paths/GrimperMur.path +++ b/src/main/deploy/pathplanner/paths/GrimperMur.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.4476034236804565, - "y": 4.85660485021398 + "x": 1.878, + "y": 5.154 }, "prevControl": null, "nextControl": { - "x": 1.6971611982881591, - "y": 4.85660485021398 + "x": 1.1275577746077026, + "y": 5.154 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.0577108433734939, - "y": 5.532096385542169 + "x": 1.0372895863052778, + "y": 5.011868758915835 }, "prevControl": { - "x": 1.4846865923035932, - "y": 5.545035044600658 + "x": 1.4642653352353772, + "y": 5.024807417974324 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/GrimperReservoir.path b/src/main/deploy/pathplanner/paths/GrimperReservoir.path index ecd7e42..e9b1621 100644 --- a/src/main/deploy/pathplanner/paths/GrimperReservoir.path +++ b/src/main/deploy/pathplanner/paths/GrimperReservoir.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.4476034236804565, - "y": 4.85660485021398 + "x": 1.878, + "y": 5.154 }, "prevControl": null, "nextControl": { - "x": 2.3958487874465044, - "y": 2.695848787446506 + "x": 1.8262453637660478, + "y": 2.993243937232526 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.0502282453637657, - "y": 2.4758915834522113 + "x": 2.0076890156918683, + "y": 2.2041797432239663 }, "prevControl": { - "x": 2.292339514978602, - "y": 2.3982596291012843 + "x": 3.2498002853067054, + "y": 2.1265477888730393 }, "nextControl": null, "isLocked": false, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 32.10625595511781 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/Tir.path b/src/main/deploy/pathplanner/paths/Tir.path index 7a9bc77..c10e78d 100644 --- a/src/main/deploy/pathplanner/paths/Tir.path +++ b/src/main/deploy/pathplanner/paths/Tir.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 0.5197432239657629, - "y": 5.930513552068473 + "x": 0.29978601997146925, + "y": 5.917574893009986 }, "prevControl": null, "nextControl": { - "x": 1.6971611982881594, - "y": 5.995206847360913 + "x": 1.4772039942938657, + "y": 5.982268188302426 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.4476034236804565, - "y": 4.85660485021398 + "x": 1.8783024251069897, + "y": 5.154194008559203 }, "prevControl": { - "x": 2.382910128388017, - "y": 5.400028530670471 + "x": 1.8136091298145502, + "y": 5.6976176890156935 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 53f2530..5ad0174 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.Aspirer; import frc.robot.commands.DescendreBalyeuse; import frc.robot.commands.DescendreGrimpeur; +import frc.robot.commands.DescendreGrimpeurPlus; import frc.robot.commands.Inverser; import frc.robot.commands.Lancer; import frc.robot.commands.LancerAspirer; @@ -45,7 +46,7 @@ import frc.robot.generated.TunerConstants; import frc.robot.subsystems.Balayeuse; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Grimpeur; -import frc.robot.subsystems.LEDSubsystem; +//import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.Lanceur; import frc.robot.subsystems.LimeLight3; import frc.robot.subsystems.Limelight3G; @@ -57,7 +58,7 @@ public class RobotContainer { Lanceur lanceur = new Lanceur(); LimeLight3 limeLight3 = new LimeLight3(); Limelight3G limeLight3G = new Limelight3G(); - LEDSubsystem ledSubsystem = new LEDSubsystem(); + //LEDSubsystem ledSubsystem = new LEDSubsystem(); CommandXboxController manette = new CommandXboxController(0); CommandXboxController manette1 = new CommandXboxController(1); private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed @@ -95,7 +96,7 @@ public class RobotContainer { } private void configureBindings() { - ledSubsystem.setDefaultCommand(ledSubsystem.updateLEDs()); + //ledSubsystem.setDefaultCommand(ledSubsystem.updateLEDs()); drivetrain.setDefaultCommand( drivetrain.applyRequest(() -> drive.withVelocityY(MathUtil.applyDeadband(-manette.getLeftX()*Math.abs(-manette.getLeftX())*MaxSpeed*0.7, 0.05)) @@ -110,7 +111,7 @@ public class RobotContainer { // manette1.povLeft().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse)); manette.povUp().whileTrue(new MonterGrimpeur(grimpeur)); - manette.povDown().whileTrue(new DescendreGrimpeur(grimpeur)); + manette.povDown().whileTrue(new DescendreGrimpeurPlus(grimpeur)); manette.rightTrigger().whileTrue(new Lancer(lanceur, limeLight3G)); manette.leftTrigger().whileTrue(new Limelighter(limeLight3G, drivetrain)); manette.leftBumper().whileTrue(new DescendreBalyeuse(balayeuse)); @@ -130,6 +131,7 @@ public class RobotContainer { manette1.y().whileTrue(new ModeOposerBalayeuse(balayeuse)); manette1.povRight().whileTrue(new BougerDroiteAuto(drivetrain)); manette1.povLeft().whileTrue(new BougerGaucheAuto(drivetrain)); + manette1.povDown().whileTrue(new DescendreGrimpeur(grimpeur)); manette1.start().whileTrue(new Inverser(drivetrain)); } diff --git a/src/main/java/frc/robot/commands/DescendreGrimpeur.java b/src/main/java/frc/robot/commands/DescendreGrimpeur.java index 67a6e9a..62dc23f 100644 --- a/src/main/java/frc/robot/commands/DescendreGrimpeur.java +++ b/src/main/java/frc/robot/commands/DescendreGrimpeur.java @@ -13,6 +13,7 @@ public class DescendreGrimpeur extends Command { /** Creates a new DescendreGrimpeur. */ public DescendreGrimpeur(Grimpeur grimpeur) { this.grimpeur = grimpeur; + addRequirements(grimpeur); // Use addRequirements() here to declare subsystem dependencies. } diff --git a/src/main/java/frc/robot/commands/DescendreGrimpeurPlus.java b/src/main/java/frc/robot/commands/DescendreGrimpeurPlus.java new file mode 100644 index 0000000..a1786ff --- /dev/null +++ b/src/main/java/frc/robot/commands/DescendreGrimpeurPlus.java @@ -0,0 +1,50 @@ +// 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.Grimpeur; + +/* 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 DescendreGrimpeurPlus extends Command { + private Grimpeur grimpeur; + /** Creates a new DescendreGrimpeur. */ + public DescendreGrimpeurPlus(Grimpeur grimpeur) { + this.grimpeur = grimpeur; + addRequirements(grimpeur); + // 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() { + if(!grimpeur.Limit()){ + grimpeur.GrimperGauche(-0.4); + grimpeur.GrimperDroit(-0.4); + } + else{ + grimpeur.Reset(); + grimpeur.GrimperGauche(0); + grimpeur.GrimperDroit(0); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + grimpeur.GrimperGauche(0); + grimpeur.GrimperDroit(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/Lancer.java b/src/main/java/frc/robot/commands/Lancer.java index ace73e9..f224f88 100644 --- a/src/main/java/frc/robot/commands/Lancer.java +++ b/src/main/java/frc/robot/commands/Lancer.java @@ -20,7 +20,7 @@ public class Lancer extends Command { private PIDController pidController; private Limelight3G limeLight3G; private Timer timer; - double vitesse = 0.5; + double vitesse = 0; double botx = 0; double boty = 0; Optional alliance = DriverStation.getAlliance(); @@ -61,9 +61,9 @@ public class Lancer extends Command { boty = BotPose[1]; vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250; } - if(limeLight3G.getV()){ + // if(limeLight3G.getV()){ - + if(vitesse > 2000){ double output = pidController.calculate(lanceur.Vitesse(),vitesse); lanceur.Lancer(output); System.out.println(output); @@ -72,9 +72,12 @@ public class Lancer extends Command { if(timer.get() >1){ lanceur.Demeler(1); } - + } + else{ + lanceur.Lancer(3500); + } } - } + // } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/Limelighter.java b/src/main/java/frc/robot/commands/Limelighter.java index 4ad8e8b..f59318e 100644 --- a/src/main/java/frc/robot/commands/Limelighter.java +++ b/src/main/java/frc/robot/commands/Limelighter.java @@ -71,48 +71,48 @@ public class Limelighter extends Command { angle = BotPose[5]; calcul = Math.toRadians(limelight3g.Calcule(boty, 4,botx, x, angle)); System.out.println(calcul); - drivetrain.setControl(drive.withRotationalRate(MathUtil.clamp(calcul, -3, 3))); - // if(calcul > -5 && calcul < 5){ - // drivetrain.setControl( - // drive.withRotationalRate(0)); - // } - // else if(calcul > 5){ - // drivetrain.setControl( - // drive.withRotationalRate(2)); - // } - // else if(calcul < -5){ - // drivetrain.setControl(drive.withRotationalRate(-2)); - // } - // // botx = BotPose[1]; - // boty = BotPose[0]; - // angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); - // calcul = limelight3g.Calcule(botx, x, boty, 4, angle); - // if(calcul < -5 && calcul > -180){ - // drivetrain.setControl( - // drive.withRotationalRate(0.5*(2*Math.PI))); - // } - // else if(calcul > 5 && calcul < 180){ - // drivetrain.setControl( - // drive.withRotationalRate(-0.5*(2*Math.PI))); - // } - // else if(calcul < -5){ - // drivetrain.setControl( - // drive.withRotationalRate(-0.5*(2*Math.PI))); - // } - // else if(calcul <= -180){ - // drivetrain.setControl( - // drive.withRotationalRate(0.5*(2*Math.PI))); - // } - // else{ - // drivetrain.setControl( - // drive.withRotationalRate(0)); - // } - // drivetrain.setControl( - // drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); - // System.out.println(angle); - // if (calcul < 0.2 && calcul > -0.2) { - // drivetrain.setControl(drive.withRotationalRate(0)); - // } + //drivetrain.setControl(drive.withRotationalRate(MathUtil.clamp(calcul, -3, 3))); + if(calcul > -5 && calcul < 5){ + drivetrain.setControl( + drive.withRotationalRate(0)); + } + else if(calcul > 5){ + drivetrain.setControl( + drive.withRotationalRate(2)); + } + else if(calcul < -5){ + drivetrain.setControl(drive.withRotationalRate(-2)); + } + botx = BotPose[1]; + boty = BotPose[0]; + angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); + calcul = limelight3g.Calcule(botx, x, boty, 4, angle); + if(calcul < -5 && calcul > -180){ + drivetrain.setControl( + drive.withRotationalRate(0.5*(2*Math.PI))); + } + else if(calcul > 5 && calcul < 180){ + drivetrain.setControl( + drive.withRotationalRate(-0.5*(2*Math.PI))); + } + else if(calcul < -5){ + drivetrain.setControl( + drive.withRotationalRate(-0.5*(2*Math.PI))); + } + else if(calcul <= -180){ + drivetrain.setControl( + drive.withRotationalRate(0.5*(2*Math.PI))); + } + else{ + drivetrain.setControl( + drive.withRotationalRate(0)); + } + drivetrain.setControl( + drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); + System.out.println(angle); + if (calcul < 0.2 && calcul > -0.2) { + drivetrain.setControl(drive.withRotationalRate(0)); + } } else{ drivetrain.setControl(drive.withRotationalRate(0)); diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java index 4e27543..03d9ecc 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java @@ -71,10 +71,10 @@ public class GrimperMur extends Command { pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); System.out.println(pigeonAngle); y = 3.6; - x = 14.9; + x = 15; angle = 180; if(pigeonAngle> 358 || pigeonAngle< 2){ - drivetrain.setControl(drive.withVelocityY(MathUtil.clamp((-(y-boty)*6.5), -2, 2)).withVelocityX(MathUtil.clamp((-(x-botx)*6.5),-2,2)).withRotationalRate(0)); + drivetrain.setControl(drive.withVelocityY(MathUtil.clamp((-(y-boty)*10), -2, 2)).withVelocityX(MathUtil.clamp((-(x-botx)*10),-2,2)).withRotationalRate(0)); } else{ if(pigeonAngle>180){ @@ -89,10 +89,10 @@ public class GrimperMur extends Command { pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); System.out.println(pigeonAngle); y = 4.4; - x = 1.7; + x = 1.6; angle = 0; if(pigeonAngle> 182 || pigeonAngle< 178){ - drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)).withRotationalRate(0)); + drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*10), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*10),-2,2)).withRotationalRate(0)); } else{ if(pigeonAngle>0 && pigeonAngle<180){ @@ -120,6 +120,6 @@ public class GrimperMur extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05); + return (y-boty < 0.2 && y-boty >-0.2) && (x-botx < 0.06 && x-botx > -0.06); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java index 6631809..244946a 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java @@ -73,7 +73,7 @@ public class GrimperReservoir extends Command { x = 15.6; angle = 180; if(pigeonAngle< 190 && pigeonAngle> 170){ - drivetrain.setControl(drive.withVelocityY(MathUtil.clamp((-(y-boty)*6.5), -2, 2)).withVelocityX(MathUtil.clamp((-(x-botx)*6.5),-2,2))); + drivetrain.setControl(drive.withVelocityY(MathUtil.clamp((-(y-boty)*10), -2, 2)).withVelocityX(MathUtil.clamp((-(x-botx)*10),-2,2))); } else{ if(pigeonAngle>180){ @@ -91,7 +91,7 @@ public class GrimperReservoir extends Command { x = 1.11; angle = 0; if(pigeonAngle> 358 || pigeonAngle< 2){ - drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)).withRotationalRate(0)); + drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*10), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*10),-2,2)).withRotationalRate(0)); } else{ if(pigeonAngle>0 && pigeonAngle<180){ @@ -119,6 +119,6 @@ public class GrimperReservoir extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return (y-boty < 0.1 && y-boty >-0.1) && (x-botx < 0.1 && x-botx > -0.1); + return (y-boty < 0.2 && y-boty >-0.2) && (x-botx < 0.06 && x-botx > -0.06); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/MonterGrimpeur.java b/src/main/java/frc/robot/commands/MonterGrimpeur.java index 6c0bd98..0ca7a08 100644 --- a/src/main/java/frc/robot/commands/MonterGrimpeur.java +++ b/src/main/java/frc/robot/commands/MonterGrimpeur.java @@ -13,6 +13,7 @@ public class MonterGrimpeur extends Command { /** Creates a new MonterGrimpeur. */ public MonterGrimpeur(Grimpeur grimpeur) { this.grimpeur = grimpeur; + addRequirements(grimpeur); // Use addRequirements() here to declare subsystem dependencies. } diff --git a/src/main/java/frc/robot/subsystems/Grimpeur.java b/src/main/java/frc/robot/subsystems/Grimpeur.java index a03b0bb..8e3f32d 100644 --- a/src/main/java/frc/robot/subsystems/Grimpeur.java +++ b/src/main/java/frc/robot/subsystems/Grimpeur.java @@ -4,10 +4,7 @@ package frc.robot.subsystems; -import com.revrobotics.PersistMode; -import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.networktables.GenericEntry; @@ -18,33 +15,44 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Grimpeur extends SubsystemBase { ShuffleboardTab teb = Shuffleboard.getTab("teb"); - SparkMax grimpeur1 = new SparkMax(3,MotorType.kBrushless); - SparkMax grimpeur2 = new SparkMax(12,MotorType.kBrushless); - - SparkMaxConfig slaveConfig = new SparkMaxConfig(); + SparkMax grimpeur1 = new SparkMax(3, MotorType.kBrushless); + SparkMax grimpeur2 = new SparkMax(12, MotorType.kBrushless); + + // SparkMaxConfig slaveConfig = new SparkMaxConfig(); DigitalInput limit = new DigitalInput(0); - private GenericEntry EncodeurGrimpeur = - teb.add("Position haut grimpeur", 100).getEntry(); - public void Grimper(double vitesse){ - grimpeur1.configure(slaveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - grimpeur2.configure(slaveConfig.follow(grimpeur1), ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + private GenericEntry EncodeurGrimpeur = teb.add("Position haut grimpeur", 100).getEntry(); + + // public void Grimper(double vitesse){ + // grimpeur1.configure(slaveConfig, ResetMode.kNoResetSafeParameters, + // PersistMode.kPersistParameters); + // grimpeur2.configure(slaveConfig.follow(grimpeur1), + // ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + // grimpeur1.set(vitesse); + // } + public void GrimperGauche(double vitesse) { grimpeur1.set(vitesse); } - public void GrimperGauche(double vitesse){ - grimpeur1.set(vitesse); + + public void GrimperDroit(double vitesse) { + grimpeur2.set(vitesse); } - public double Position(){ + + public double Position() { return grimpeur1.getEncoder().getPosition(); } - public void Reset(){ + + public void Reset() { grimpeur1.getEncoder().setPosition(0); } - public boolean Limit(){ + + public boolean Limit() { return limit.get(); } - public double PositionFinal(){ + + public double PositionFinal() { return EncodeurGrimpeur.getDouble(100); } + /** Creates a new Grimpeur. */ public Grimpeur() { teb.addDouble("encodeur grimpeur", this::Position); diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 7506c38..2ef7110 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -1,149 +1,149 @@ -/* Generated by Phoenix Tuner X */ -package frc.robot.subsystems; +// /* Generated by Phoenix Tuner X */ +// package frc.robot.subsystems; -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +// import edu.wpi.first.networktables.GenericEntry; +// import edu.wpi.first.wpilibj.DriverStation; +// import edu.wpi.first.wpilibj.Timer; +// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +// import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +// import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.SubsystemBase; -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.controls.SolidColor; -import com.ctre.phoenix6.hardware.CANdle; -import com.ctre.phoenix6.signals.RGBWColor; +// import com.ctre.phoenix6.CANBus; +// import com.ctre.phoenix6.controls.SolidColor; +// import com.ctre.phoenix6.hardware.CANdle; +// import com.ctre.phoenix6.signals.RGBWColor; -/** - * Subsystem that controls an addressable LED strip using a CANdle. - */ -public class LEDSubsystem extends SubsystemBase { - Timer _timer; - boolean q = true; - ShuffleboardTab teb = Shuffleboard.getTab("teb"); - private GenericEntry equipe = - teb.add("equipe commence (bleu = ouvert, rouge = fermé)", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry(); - private final CANBus kCANBus = new CANBus("rio"); - private final CANdle m_candle = new CANdle(17, kCANBus); - public void Bleu(){ - m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(255, 0, 0, 0))); - } - public void Rouge(){ - m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(0, 0, 255, 0))); - } - public void Vert(){ - m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(0, 255, 0, 0))); - } - public void Noir(){ +// /** +// * Subsystem that controls an addressable LED strip using a CANdle. +// */ +// public class LEDSubsystem extends SubsystemBase { +// Timer _timer; +// boolean q = true; +// ShuffleboardTab teb = Shuffleboard.getTab("teb"); +// private GenericEntry equipe = +// teb.add("equipe commence (bleu = ouvert, rouge = fermé)", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry(); +// private final CANBus kCANBus = new CANBus("rio"); +// private final CANdle m_candle = new CANdle(17, kCANBus); +// public void Bleu(){ +// m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(255, 0, 0, 0))); +// } +// public void Rouge(){ +// m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(0, 0, 255, 0))); +// } +// public void Vert(){ +// m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(0, 255, 0, 0))); +// } +// public void Noir(){ - m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(0, 0, 0, 0))); - } - public void Flash(boolean couleur){ - Timer timer = new Timer(); - if(timer.get() <0.5){ - if(couleur){ - Bleu(); - } - else{ - Rouge(); - } - } - else{ - Noir(); - timer.reset(); - } - } - public LEDSubsystem() { - setDefaultCommand(updateLEDs()); - _timer = new Timer(); - _timer.start(); - } - public boolean Equipe(){ - return equipe.getBoolean(false); - } - /** - * Updates the animations and LEDs of the CANdle. - * - * @return Command to run - */ - public Command updateLEDs() { - return run(() -> { - // if(q){ - // _timer.reset(); - // q = false; - // } - double temps = _timer.get(); - System.out.println(temps); - if(Equipe()){ - if(temps > 30){ - Vert(); - } - else if(temps > 52){ +// m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(0, 0, 0, 0))); +// } +// public void Flash(boolean couleur){ +// Timer timer = new Timer(); +// if(timer.get() <0.5){ +// if(couleur){ +// Bleu(); +// } +// else{ +// Rouge(); +// } +// } +// else{ +// Noir(); +// timer.reset(); +// } +// } +// public LEDSubsystem() { +// setDefaultCommand(updateLEDs()); +// _timer = new Timer(); +// _timer.start(); +// } +// public boolean Equipe(){ +// return equipe.getBoolean(false); +// } +// /** +// * Updates the animations and LEDs of the CANdle. +// * +// * @return Command to run +// */ +// public Command updateLEDs() { +// return run(() -> { +// // if(q){ +// // _timer.reset(); +// // q = false; +// // } +// double temps = _timer.get(); +// System.out.println(temps); +// if(Equipe()){ +// if(temps > 30){ +// Vert(); +// } +// else if(temps > 52){ - Bleu(); - } - else if(temps > 55){ - Flash(true); - } - else if(temps > 67){ - Rouge(); - } - else if(temps > 70){ - Flash(false); - } - else if(temps > 103){ - Bleu(); - } - else if(temps > 105){ - Flash(true); - } - else if(temps > 127){ - Rouge(); - } - else if(temps > 130){ - Flash(false); - } - else if(temps < 140){ - Vert(); - } - } - else{ - if(temps > 110){ - Vert(); - } - else if(temps > 88){ +// Bleu(); +// } +// else if(temps > 55){ +// Flash(true); +// } +// else if(temps > 67){ +// Rouge(); +// } +// else if(temps > 70){ +// Flash(false); +// } +// else if(temps > 103){ +// Bleu(); +// } +// else if(temps > 105){ +// Flash(true); +// } +// else if(temps > 127){ +// Rouge(); +// } +// else if(temps > 130){ +// Flash(false); +// } +// else if(temps < 140){ +// Vert(); +// } +// } +// else{ +// if(temps > 110){ +// Vert(); +// } +// else if(temps > 88){ - Rouge(); - } - else if(temps > 85){ - Flash(false); - } - else if(temps > 63){ - Bleu(); - } - else if(temps > 60){ - Flash(true); - } - else if(temps > 33){ - Rouge(); - } - else if(temps > 30){ - Flash(false); - } - else if(temps > 13){ - Bleu(); - } - else if(temps > 10){ - Flash(true); - } - else if(temps < 10){ - Vert(); - } - } - // _timer.stop(); +// Rouge(); +// } +// else if(temps > 85){ +// Flash(false); +// } +// else if(temps > 63){ +// Bleu(); +// } +// else if(temps > 60){ +// Flash(true); +// } +// else if(temps > 33){ +// Rouge(); +// } +// else if(temps > 30){ +// Flash(false); +// } +// else if(temps > 13){ +// Bleu(); +// } +// else if(temps > 10){ +// Flash(true); +// } +// else if(temps < 10){ +// Vert(); +// } +// } +// // _timer.stop(); - }); - } -} +// }); +// } +// }