Merge branch 'LED' into amp

simgui-ds.json

@@ -92,6 +92,10 @@
   "robotJoysticks": [
     {
       "guid": "Keyboard0"
+    },
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
     }
   ]
 }

simgui.json

@@ -1,4 +1,14 @@
 {
+  "HALProvider": {
+    "Addressable LEDs": {
+      "0": {
+        "columns": 44
+      },
+      "window": {
+        "visible": true
+      }
+    }
+  },
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
@@ -36,5 +46,8 @@
   },
   "NetworkTables Info": {
     "visible": true
+  },
+  "NetworkTables View": {
+    "visible": false
   }
 }

src/main/java/frc/robot/Constants.java

@@ -159,4 +159,10 @@ public static final class VisionConstants {
     public static final double kFieldLength = 16.54175;
   }
 
+  public static final class LEDConstants {
+    // TODO: find plz
+    public static final int kLEDPort = 0;
+    public static final int kLEDLength = 97;
+  }
+
 }

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

@@ -28,13 +28,15 @@
 import frc.robot.Constants.IOConstants;
 import frc.robot.Constants.ShooterConstants;
 import frc.robot.commands.AmpOuttakeCommand;
+import frc.robot.commands.DefaultLEDCommand;
 import frc.robot.commands.IntakeArmPositionCommand;
 import frc.robot.commands.NoteIntakeCommand;
 import frc.robot.commands.NoteOuttakeCommand;
 import frc.robot.commands.ShooterSetSpeedCommand;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.LEDSubsystem;
 import frc.robot.subsystems.IntakeSubsystem.ArmPosition;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.ShooterSubsystem.ShootSpeed;
@@ -53,6 +55,7 @@ public class RobotContainer {
   private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
   private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
   private final VisionSubsystem m_visionSubsystem = new VisionSubsystem();
+  private final LEDSubsystem m_ledSubsystem = new LEDSubsystem();
 
   private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
   private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort);
@@ -131,6 +134,10 @@ public RobotContainer() {
     // Configure the trigger bindings
     configureBindings();
 
+    m_ledSubsystem.setDefaultCommand(
+      new DefaultLEDCommand(m_ledSubsystem, m_intakeSubsystem, m_shooterSubsystem)
+    );
+
     m_robotDrive.setDefaultCommand(
         new RunCommand(
             () -> m_robotDrive.drive(

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

@@ -0,0 +1,79 @@
+// 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.Constants.IntakeConstants;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.LEDSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+
+public class DefaultLEDCommand extends Command {
+  private LEDSubsystem m_ledSubsystem;
+  private IntakeSubsystem m_intakeSubsystem;
+  private ShooterSubsystem m_shooterSubsystem;
+
+  private double armSetpoint;
+  private double shootSpeed;
+  private int[] rgb = new int[3];
+
+  /** Creates a new SetLED. */
+  public DefaultLEDCommand(LEDSubsystem LED, IntakeSubsystem intake, ShooterSubsystem shooter) {
+    m_ledSubsystem = LED;
+    m_intakeSubsystem = intake;
+    m_shooterSubsystem = shooter;
+    addRequirements(m_ledSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    // default color is invalid so the subsystem will set it to rainbow
+    rgb[0] = 256;
+    rgb[1] = 256;
+    rgb[2] = 256;
+
+    armSetpoint = m_intakeSubsystem.getArmPosition();
+    if (armSetpoint == IntakeConstants.kIntakeLoweredAngle) {
+      rgb[0] = 255;
+      rgb[1] = 0;
+      rgb[2] = 0;
+    } else if (armSetpoint == IntakeConstants.kIntakeRaisedAngle && m_intakeSubsystem.haveNote()) {
+      rgb[0] = 0;
+      rgb[1] = 0;
+      rgb[2] = 255;
+    }
+
+    shootSpeed = m_shooterSubsystem.returnCurrentSpeed();
+    if (shootSpeed > 500 && shootSpeed < 1899 * Math.PI) { // if charging up
+      rgb[0] = 150;
+      rgb[1] = 150;
+      rgb[2] = 0;
+    } else if (shootSpeed > 1899 * Math.PI) { // if the shooter is ready to shoot
+      rgb[0] = 0;
+      rgb[1] = 255;
+      rgb[2] = 0;
+    }
+
+    m_ledSubsystem.setLED(rgb[0], rgb[1], rgb[2]);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

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

@@ -199,6 +199,9 @@ public void resetOdometry(Pose2d pose) {
         },
         pose);
   }
+  public Pose2d getOdometry(){
+    return m_poseEstimator.getEstimatedPosition();
+  }
 
   /** Zeroes the heading of the robot. */
   public void zeroHeading() {

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

@@ -0,0 +1,69 @@
+// 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 edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.LEDConstants;
+
+public class LEDSubsystem extends SubsystemBase {
+  private final AddressableLED m_LED = new AddressableLED(LEDConstants.kLEDPort);
+  private final AddressableLEDBuffer m_LEDBuffer = new AddressableLEDBuffer(LEDConstants.kLEDLength);
+  private int m_rainbowFirstPixelHue = 0;
+
+  /** Creates a new {@link LEDSubsystem}. */
+  public LEDSubsystem() {
+    m_LED.setLength(LEDConstants.kLEDLength); // 29
+    m_LED.setData(m_LEDBuffer);
+    m_LED.start();
+
+    // Yellow when bot turns on.
+    setLED(50, 50, 0);
+
+    SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString());
+  }
+
+  /**
+   * Sets the rgb value for all LEDs.
+   * 
+   * @param r Red 0-255
+   * @param g Green 0-255
+   * @param b Blue 0-255
+   */
+  public void setLED(int r, int g, int b) {
+
+    // If we get an invalid value just set it to a rainbow
+    if (r > 255 || b > 255 || g > 255) {
+      rainbow();
+      return;
+    }
+
+    for (var i = 0; i < LEDConstants.kLEDLength; i++) {
+      m_LEDBuffer.setRGB(i, r, g, b);
+    }
+    m_LED.setData(m_LEDBuffer);
+    SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString());
+  }
+
+  private void rainbow() {
+    // For every pixel
+    for (var i = 0; i < m_LEDBuffer.getLength(); i++) {
+      // Calculate the hue - hue is easier for rainbows because the color
+      // shape is a circle so only one value needs to precess
+      final var hue = (m_rainbowFirstPixelHue + (i * 180 / m_LEDBuffer.getLength())) % 180;
+      // Set the value
+      m_LEDBuffer.setHSV(i, hue, 255, 128);
+    }
+    // Increase by to make the rainbow "move"
+    m_rainbowFirstPixelHue += 2;
+    // Check bounds
+    m_rainbowFirstPixelHue %= 180;
+
+    m_LED.setData(m_LEDBuffer);
+    SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString());
+  }
+}

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

@@ -5,11 +5,13 @@
 package frc.robot.subsystems;
 
 import com.revrobotics.CANSparkBase.IdleMode;
+
 import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Robot;
 import frc.robot.Constants.ShooterConstants;
 
 public class ShooterSubsystem extends SubsystemBase {
@@ -20,6 +22,8 @@ public class ShooterSubsystem extends SubsystemBase {
   private double m_topSpeed = 0;
   private double m_bottomSpeed = 0;
 
+  private double m_simRPM = 0;
+
   public ShooterSubsystem() {
     m_bottom.setIdleMode(IdleMode.kCoast);
     m_top.setIdleMode(IdleMode.kCoast);
@@ -57,14 +61,16 @@ public void setShootingSpeed(ShootSpeed speed) {
   }
 
   public double returnCurrentSpeed() {
-    return m_bottom.getEncoder().getVelocity();
+    if (m_topSpeed > 0.5 && m_simRPM < 15) m_simRPM++;
+    else if (m_simRPM > 0) m_simRPM--;
+    return Robot.isReal() ? m_bottom.getEncoder().getVelocity() : m_simRPM * 400;
   }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
     // SmartDashboard.putNumber("bottom Speed", m_bottomSpeed);
-    // SmartDashboard.putNumber("top Speed", m_topSpeed);
+    SmartDashboard.putNumber("top Speed", m_bottom.getEncoder().getVelocity()); 
 
     m_bottom.set(m_bottomSpeed);
     m_top.set(m_topSpeed);