Add LED's to display robot state

src/main/java/frc/robot/Robot.java

@@ -12,6 +12,8 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DataLogManager;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -95,6 +97,9 @@ private void configureBindings() {
 
   @SuppressWarnings("removal")
   public Robot() {
+    DataLogManager.start();
+    DriverStation.startDataLog(DataLogManager.getLog());
+
     m_driver = new CommandXboxController(Constants.driverport);
     m_codriver = new CommandXboxController(Constants.codriverport);
 

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

@@ -7,6 +7,10 @@
 import com.revrobotics.CANSparkMax;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.util.datalog.DataLog;
+import edu.wpi.first.util.datalog.DoubleLogEntry;
+import edu.wpi.first.util.datalog.StringLogEntry;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -28,6 +32,15 @@ public enum Setpoint {
   private CANSparkMax m_primaryMotor;
   private DutyCycleEncoder m_encoder;
   private ArmFeedforward m_feedforward;
+  private DataLog m_log = DataLogManager.getLog();
+  private DoubleLogEntry log_pid_output = new DoubleLogEntry(m_log, "/arm/pid/output");
+  private DoubleLogEntry log_pid_setpoint = new DoubleLogEntry(m_log, "/arm/pid/setpoint");
+  private DoubleLogEntry log_ff_position_setpoint =
+      new DoubleLogEntry(m_log, "/arm/ff/position_setpoint");
+  private DoubleLogEntry log_ff_velocity_setpoint =
+      new DoubleLogEntry(m_log, "/arm/ff/velocity_setpoint");
+  private DoubleLogEntry log_ff_output = new DoubleLogEntry(m_log, "/arm/ff/output");
+  private StringLogEntry log_setpoint = new StringLogEntry(m_log, "/arm/setpoint");
 
   /**
    * Creates a new {@link Arm} {@link edu.wpi.first.wpilibj2.command.Subsystem}.
@@ -47,11 +60,16 @@ public Arm(CANSparkMax primaryMotor) {
   /** Achieves and maintains speed for the primary motor. */
   private Command achievePosition(double position) {
     return Commands.run(
-        () ->
-            m_primaryMotor.setVoltage(
-                -1
-                    * (m_feedforward.calculate(position, (5676 / 250))
-                        + m_pid.calculate(m_encoder.getAbsolutePosition() * 2 * 3.14, position))));
+        () -> {
+          var pid_output = m_pid.calculate(m_encoder.getAbsolutePosition() * 2 * 3.14, position);
+          log_pid_output.append(pid_output);
+          log_pid_setpoint.append(m_pid.getSetpoint());
+          var ff_output = m_feedforward.calculate(position, (5676 / 250));
+          log_ff_output.append(ff_output);
+          log_ff_position_setpoint.append(position);
+          log_ff_velocity_setpoint.append((5676 / 250));
+          m_primaryMotor.setVoltage(-1 * ff_output + pid_output);
+        });
   }
 
   public Command stop() {
@@ -101,10 +119,12 @@ public Command manualControl(DoubleSupplier speed) {
    * Makes the arm go to a setpoint from the {@link Setpoint} enum
    *
    * @param setpoint The setpoint to go to, a {@link Setpoint}
+   *
    * @return A {@link Command} to go to the setpoint
    */
   public Command goToSetpoint(Setpoint setpoint) {
     double position = 0;
+    log_setpoint.append(setpoint.name());
 
     switch (setpoint) {
       case kAmp:

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

@@ -8,6 +8,9 @@
 import com.revrobotics.RelativeEncoder;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.util.datalog.DataLog;
+import edu.wpi.first.util.datalog.DoubleLogEntry;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -18,6 +21,8 @@ public class Climber extends SubsystemBase {
   private PIDController m_pid;
   private CANSparkMax m_motor;
   private RelativeEncoder m_encoder;
+  private DataLog m_log = DataLogManager.getLog();
+  private DoubleLogEntry log_pid_output = new DoubleLogEntry(m_log, "/climber/pid/output");
 
   /**
    * Creates a new {@link Climber} {@link edu.wpi.first.wpilibj2.command.Subsystem}.
@@ -38,8 +43,11 @@ public Climber(CANSparkMax motor) {
    */
   public Command climb() {
     return Commands.run(
-        () ->
-            m_motor.setVoltage(
-                m_pid.calculate(Units.rotationsToRadians(m_encoder.getPosition() * -1), -3.14159)));
+        () -> {
+          var output =
+              m_pid.calculate(Units.rotationsToRadians(m_encoder.getPosition() * -1), -3.14159);
+          log_pid_output.append(output);
+          m_motor.setVoltage(output);
+        });
   }
 }

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

@@ -5,19 +5,32 @@
 package frc.robot.subsystems;
 
 import com.revrobotics.CANSparkMax;
+import edu.wpi.first.util.datalog.DataLog;
+import edu.wpi.first.util.datalog.DoubleLogEntry;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj.PWM;
 
 public class Intake extends SubsystemBase {
   private CANSparkMax m_motor;
+  private DataLog log = DataLogManager.getLog();
+  private DoubleLogEntry log_output = new DoubleLogEntry(log, "/intake/output");
+
+  PWM lEDPwm = new PWM(0);
 
   public Intake(CANSparkMax motor) {
     m_motor = motor;
   }
 
   public Command intake() {
-    return Commands.run(() -> m_motor.setVoltage(4))
+    lEDPwm.setSpeed(0.59); // dark red //
+    return Commands.run(
+            () -> {
+              log_output.append(4);
+              m_motor.setVoltage(4);
+            })
         .withTimeout(2)
         .andThen(runOnce(() -> m_motor.setVoltage(0)));
   }
@@ -27,7 +40,12 @@ public Command stop() {
   }
 
   public Command outake() {
-    return Commands.run(() -> m_motor.setVoltage(-4))
+    lEDPwm.setSpeed(0.63); // red orange, 0.65 for normal orange //
+    return Commands.run(
+            () -> {
+              log_output.append(-4);
+              m_motor.setVoltage(-4);
+            })
         .withTimeout(4)
         .andThen(runOnce(() -> m_motor.setVoltage(0)));
   }

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

@@ -8,18 +8,25 @@
 import com.revrobotics.RelativeEncoder;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.util.datalog.DataLog;
+import edu.wpi.first.util.datalog.DoubleLogEntry;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants;
+import edu.wpi.first.wpilibj.PWM;
 
 /** Our Crescendo shooter Subsystem */
 public class Shooter extends SubsystemBase {
   private PIDController m_pid;
   private CANSparkMax m_motor;
   private RelativeEncoder m_encoder;
-
+  private DataLog m_log = DataLogManager.getLog();
+  private DoubleLogEntry log_pid_output = new DoubleLogEntry(m_log, "/shooter/pid/output");
+
+  PWM lEDPwm = new PWM(0);
   /**
    * Creates a new {@link Shooter} {@link edu.wpi.first.wpilibj2.command.Subsystem}.
    *
@@ -36,10 +43,13 @@ private Command achieveSpeeds(double speed) {
     m_pid.reset();
     m_pid.setSetpoint(speed);
     return Commands.run(
-        () ->
-            m_motor.setVoltage(
-                m_pid.calculate(
-                    -1 * Units.rotationsPerMinuteToRadiansPerSecond(m_encoder.getVelocity()))));
+        () -> {
+          var output =
+              m_pid.calculate(
+                  -1 * Units.rotationsPerMinuteToRadiansPerSecond(m_encoder.getVelocity()));
+          log_pid_output.append(output);
+          m_motor.setVoltage(output);
+        });
   }
 
   /**
@@ -49,10 +59,12 @@ private Command achieveSpeeds(double speed) {
    * @return a {@link Command} to get to the desired speed.
    */
   public Command spinup(double speed) {
+    lEDPwm.setSpeed(0.69); // yellow //
     return achieveSpeeds(speed).until(m_pid::atSetpoint);
   }
 
   public Command stop() {
+    lEDPwm.setSpeed(0.73); // lime //
     return runOnce(() -> m_motor.set(0));
   }
 
@@ -62,6 +74,7 @@ public Command stop() {
    * @return A {@link Command} to hold the speed at the setpoint.
    */
   public Command maintain() {
+    lEDPwm.setSpeed(0.57); // hot pink //
     return achieveSpeeds(m_pid.getSetpoint());
   }