diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f4d6ba0..60e17fe 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -45,8 +45,8 @@ 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.Lanceur; -import frc.robot.subsystems.Led; import frc.robot.subsystems.LimeLight3; import frc.robot.subsystems.Limelight3G; @@ -57,7 +57,7 @@ public class RobotContainer { Lanceur lanceur = new Lanceur(); LimeLight3 limeLight3 = new LimeLight3(); Limelight3G limeLight3G = new Limelight3G(); - Led led = new Led(); + 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 @@ -67,10 +67,6 @@ public class RobotContainer { private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors - private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - - private final Telemetry logger = new Telemetry(MaxSpeed); public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); @@ -99,6 +95,7 @@ public class RobotContainer { } private void configureBindings() { + ledSubsystem.setDefaultCommand(ledSubsystem.updateLEDs()); drivetrain.setDefaultCommand( drivetrain.applyRequest(() -> drive.withVelocityY(MathUtil.applyDeadband(-manette.getLeftX()*Math.abs(-manette.getLeftX())*MaxSpeed*0.7, 0.05)) diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index c20b5f1..e04f11f 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -22,14 +22,10 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; -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.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -58,10 +54,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su /* Swerve requests to apply during SysId characterization */ private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - ShuffleboardTab teb = Shuffleboard.getTab("teb"); private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - private GenericEntry equipe = - teb.add("equipe commence (bleu = ouvert, rouge = fermé)", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry(); /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( new SysIdRoutine.Config( @@ -227,9 +220,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su } configureAutoBuilder(); } - public boolean Equipe(){ - return equipe.getBoolean(false); - } + /** * Returns a command that applies the specified control request to this swerve drivetrain. * diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 9bb2c7d..4ec5745 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -1,8 +1,12 @@ /* Generated by Phoenix Tuner X */ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.*; - +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; @@ -15,6 +19,9 @@ import com.ctre.phoenix6.signals.RGBWColor; * Subsystem that controls an addressable LED strip using a CANdle. */ public class LEDSubsystem extends SubsystemBase { + 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(){ @@ -23,17 +30,108 @@ public class LEDSubsystem extends SubsystemBase { public void Rouge(){ new SolidColor(0, 80).withColor(new RGBWColor(0, 0, 255, 0)); } + public void Vert(){ + new SolidColor(0, 80).withColor(new RGBWColor(0, 255, 0, 0)); + } + public void Noir(){ + + new SolidColor(0, 80).withColor(new RGBWColor(0, 0, 0, 0)); + } + public void Flach(boolean couleur){ + Timer timer = new Timer(); + timer.start(); + if(timer.get() <0.5){ + if(couleur){ + Bleu(); + } + else{ + Rouge(); + } + } + else{ + Noir(); + timer.reset(); + } + } public LEDSubsystem() { setDefaultCommand(updateLEDs()); } - + public boolean Equipe(){ + return equipe.getBoolean(false); + } /** * Updates the animations and LEDs of the CANdle. * * @return Command to run */ public Command updateLEDs() { - return run(() -> {}); + double temps = DriverStation.getMatchTime(); + return run(() -> { + if(Equipe()){ + if(temps < 30){ + Vert(); + } + else if(temps < 52){ + Bleu(); + } + else if(temps < 55){ + Flach(true); + } + else if(temps < 77){ + Rouge(); + } + else if(temps < 80){ + Flach(false); + } + else if(temps < 102){ + Bleu(); + } + else if(temps < 105){ + Flach(true); + } + else if(temps <127){ + Rouge(); + } + else if(temps < 130){ + Flach(false); + } + else if(temps < 140){ + Vert(); + } + } + else{ + if(temps < 30){ + Vert(); + } + else if(temps < 52){ + Rouge(); + } + else if(temps < 55){ + Flach(false); + } + else if(temps < 77){ + Bleu(); + } + else if(temps < 80){ + Flach(true); + } + else if(temps < 102){ + Rouge(); + } + else if(temps < 105){ + Flach(false); + } + else if(temps <127){ + Bleu(); + } + else if(temps < 130){ + Flach(true); + } + else if(temps < 140){ + Vert(); + } + } + }); } } diff --git a/src/main/java/frc/robot/subsystems/Led.java b/src/main/java/frc/robot/subsystems/Led.java deleted file mode 100644 index da22522..0000000 --- a/src/main/java/frc/robot/subsystems/Led.java +++ /dev/null @@ -1,135 +0,0 @@ -// 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.subsystems; - -import com.ctre.phoenix.led.CANdle; -import com.ctre.phoenix.led.RainbowAnimation; - -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.DriverStation; -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.SubsystemBase; - -public class Led extends SubsystemBase { - ShuffleboardTab teb = Shuffleboard.getTab("teb"); - CANdle CANDle = new CANdle(17); - RainbowAnimation rainbowAnim = new RainbowAnimation(); - public void bleu(){ - 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); - - } - @SuppressWarnings("removal") - public void Vert1(){ - 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,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); - } - @SuppressWarnings("removal") - public void Rouge2(){ - 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); - // CANDle.setLEDs(255, 0, 0,0,72,8); - // CANDle.setLEDs(255, 0, 0,0,88,8); - // CANDle.setLEDs(255, 0, 0,0,104,8); - // CANDle.setLEDs(255, 0, 0,0,120,8); - // CANDle.setLEDs(255, 0, 0,0,136,8); - } - @SuppressWarnings("removal") - public void RainBow(){ - CANDle.animate(rainbowAnim); - } - @SuppressWarnings("removal") - public void RainBowStop(){ - CANDle.animate(null); - } - /** Creates a new Led. */ - public Led() {} - - @Override - public void periodic() { - - double temps = DriverStation.getMatchTime(); - if(temps > 20 && temps < 30){ - Vert1(); - } - // if(Equipe()){ - if(temps > 30 && temps < 55){ - bleu(); - } - else if(temps > 30 && temps < 80){ - Rouge(); - } - else if(temps > 80 && temps < 105){ - bleu(); - } - else if(temps > 105 && temps < 130){ - Rouge(); - } - else{ - RainBow(); - } - // } - // else{ - if(temps > 30 && temps < 55){ - Rouge(); - } - else if(temps > 30 && temps < 80){ - bleu(); - } - else if(temps > 80 && temps < 105){ - Rouge(); - } - else if(temps > 105 && temps < 130){ - bleu(); - } - else{ - RainBow(); - } - } - // This method will be called once per scheduler run - - // } - -}