Merge pull request #3 from Prospect-Robotics/vdikov/akit-replay

Fix REPLAY of arm and elevator mech vizualizations. The IO values are…

Author: spderman3333

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

@@ -21,7 +21,7 @@
  * (log replay from a file).
  */
 public final class Constants {
-    public static final Mode simMode = Mode.SIM;
+    public static final Mode simMode = Mode.REPLAY;
     public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode;
 
     public enum Mode {

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

@@ -111,7 +111,7 @@ public void robotPeriodic() {
         // This must be called from the robot's periodic block in order for anything in
         // the Command-based framework to work.
         CommandScheduler.getInstance().run();
-        if (Robot.isSimulation()) {
+        if (Constants.currentMode != Constants.Mode.REAL) {
             SimulationVisualizer.getInstance().periodic();
         }
 

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

@@ -23,12 +23,15 @@ public Arm(ArmIO io) {
     @Override
     public void periodic() {
         io.updateState(replayedInputs);
+        // `processInputs` must be called every periodic after updating hardware state.
+        // In `REPLAY` mode, `updateState` does nothing, and the `replayedInputs` are populated from the replayed logs
+        // instead.
         Logger.processInputs("Arm", replayedInputs);
     }
 
     @Override
     public void simulationPeriodic() {
-        SimulationVisualizer.getInstance().updateArmRotation(io.getArmPosition());
+        SimulationVisualizer.getInstance().updateArmRotation(Degrees.of(replayedInputs.armPositionDegrees));
     }
 
     public void setArmPosition(ArmPositions armPosition) {

src/main/java/frc/robot/subsystems/elevator/Elevator.java

@@ -1,6 +1,8 @@
 package frc.robot.subsystems.elevator;
 
-import static edu.wpi.first.units.Units.*;
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.Volts;
 
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Distance;
@@ -33,7 +35,9 @@ public void stopElevator() {
     @Override
     public void periodic() {
         io.updateState(replayedInputs);
-        // Must be called every periodic after updating hardware state.
+        // `processInputs` must be called every periodic after updating hardware state.
+        // In `REPLAY` mode, `updateState` does nothing, and the `replayedInputs` are populated from the replayed logs
+        // instead.
         Logger.processInputs("Elevator", replayedInputs);
         Logger.recordOutput(
                 "Elevator/Desired Carriage Position",
@@ -42,7 +46,7 @@ public void periodic() {
 
     @Override
     public void simulationPeriodic() {
-        SimulationVisualizer.getInstance().updateElevatorHeight(io.getCarriagePosition());
+        SimulationVisualizer.getInstance().updateElevatorHeight(Inches.of(replayedInputs.carriagePositionInches));
     }
 
     public void setElevatorPosition(ElevatorHeight height) {