Added LED
This commit is contained in:
parent
0b53a79d2e
commit
4fede0ef59
@ -12,4 +12,9 @@ package frc.robot;
|
||||
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
|
||||
* constants are needed, to reduce verbosity.
|
||||
*/
|
||||
public final class Constants {}
|
||||
public final class Constants {
|
||||
public static final int MOTEUR_DROIT = 2;
|
||||
public static final int MOTEUR_GAUCHE = 1;
|
||||
|
||||
public static final int LED_PORT = 0;
|
||||
}
|
||||
|
@ -11,10 +11,12 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
import static frc.robot.Constants.*;
|
||||
|
||||
public class DriveTrain extends SubsystemBase {
|
||||
|
||||
private final WPI_TalonSRX roueGauche = new WPI_TalonSRX(1);
|
||||
private final WPI_TalonSRX roueDroite = new WPI_TalonSRX(2);
|
||||
private final WPI_TalonSRX roueGauche = new WPI_TalonSRX(MOTEUR_GAUCHE);
|
||||
private final WPI_TalonSRX roueDroite = new WPI_TalonSRX(MOTEUR_DROIT);
|
||||
|
||||
private final DifferentialDrive drive = new DifferentialDrive(roueGauche, roueDroite);
|
||||
|
||||
|
90
src/main/java/frc/robot/subsystems/Led.java
Normal file
90
src/main/java/frc/robot/subsystems/Led.java
Normal file
@ -0,0 +1,90 @@
|
||||
// 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 java.util.Map;
|
||||
|
||||
import edu.wpi.first.wpilibj.AddressableLED;
|
||||
import edu.wpi.first.wpilibj.AddressableLEDBuffer;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
import static frc.robot.Constants.*;
|
||||
|
||||
public class Led extends SubsystemBase {
|
||||
|
||||
private final ShuffleboardTab tab = Shuffleboard.getTab("Peanut");
|
||||
private final ShuffleboardLayout layout = tab.getLayout("LED", BuiltInLayouts.kList)
|
||||
.withSize(2, 2)
|
||||
.withProperties(Map.of("Label position", "HIDDEN"));
|
||||
|
||||
private final AddressableLED led = new AddressableLED(LED_PORT);
|
||||
private final AddressableLEDBuffer buffer = new AddressableLEDBuffer(150);
|
||||
|
||||
private int rainbowFirstPixelHue = 0;
|
||||
private int cycleHue = 0;
|
||||
private int chaseFirstPixel = 0;
|
||||
|
||||
/** Creates a new Led. */
|
||||
public Led() {
|
||||
led.setLength(buffer.getLength());
|
||||
led.setData(buffer);
|
||||
led.start();
|
||||
|
||||
layout.add(new RunCommand(() -> rainbow(), this));
|
||||
layout.add(new RunCommand(() -> colorCycle(), this));
|
||||
layout.add(new RunCommand(() -> chase(), this));
|
||||
}
|
||||
|
||||
public void rainbow() {
|
||||
for (int i = 0; i < buffer.getLength(); i++) {
|
||||
final int hue = (rainbowFirstPixelHue + (i * 180 / buffer.getLength())) % 180;
|
||||
buffer.setHSV(i, hue, 255, 128);
|
||||
}
|
||||
rainbowFirstPixelHue += 3;
|
||||
rainbowFirstPixelHue %= 181;
|
||||
}
|
||||
|
||||
public void colorCycle() {
|
||||
for (int i = 0; i < buffer.getLength(); i++) {
|
||||
buffer.setHSV(i, cycleHue, 255, 128);
|
||||
}
|
||||
cycleHue += 2;
|
||||
cycleHue %= 181;
|
||||
}
|
||||
|
||||
public void chase() {
|
||||
int lastPixel = (chaseFirstPixel + buffer.getLength()/15) % buffer.getLength();
|
||||
if (lastPixel < chaseFirstPixel) {
|
||||
for (int i = 0; i < buffer.getLength(); i++) {
|
||||
if (i < lastPixel || i >= chaseFirstPixel) {
|
||||
buffer.setHSV(i, 180, 255, 255);
|
||||
} else {
|
||||
buffer.setHSV(i, 180, 0, 0);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
for (int i = 0; i < buffer.getLength(); i++) {
|
||||
if (i >= chaseFirstPixel && i < lastPixel) {
|
||||
buffer.setHSV(i, 180, 255, 255);
|
||||
} else {
|
||||
buffer.setHSV(i, 180, 0, 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
chaseFirstPixel += 2;
|
||||
chaseFirstPixel %= 181;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
led.setData(buffer);
|
||||
}
|
||||
}
|
Loading…
x
Reference in New Issue
Block a user