Improved subsystem state logging

Author: sophiep29

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

@@ -4,8 +4,6 @@
 
 package frc.robot;
 
-import static frc.robot.RobotContainer.shooterState;
-
 import edu.wpi.first.epilogue.Epilogue;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -19,9 +17,13 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.commands.DrivetrainCommand;
 import frc.robot.commands.FlywheelCommand;
+import frc.robot.commands.KickerCommand;
 import frc.robot.config.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.Hopper;
+import frc.robot.subsystems.Intake;
 
 @Logged
 public class Robot extends TimedRobot {
@@ -48,8 +50,11 @@ public Robot() {
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
-    SmartDashboard.putString("shoot-state", shooterState);
-    SmartDashboard.putString("shoot-on", FlywheelCommand.isOn);
+    SmartDashboard.putString("Flywheel State", FlywheelCommand.flywheelState);
+    SmartDashboard.putString("Drivetrain State", DrivetrainCommand.drivetrainState);
+    SmartDashboard.putString("Hopper State", Hopper.hopperState);
+    SmartDashboard.putString("Intake State", Intake.intakeState);
+    SmartDashboard.putString("Kicker State", KickerCommand.kickerState);
     try {
       var pose = drivetrain.getState().Pose;
       SmartDashboard.putNumber("OdometryX", pose.getX());

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

@@ -31,6 +31,7 @@ public static enum Position {
   private DoubleSupplier leftY;
   private DoubleSupplier leftX;
   private DoubleSupplier rightX;
+  public static String drivetrainState = "Stopped";
 
   private final double MaxSpeed =
       TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
@@ -113,25 +114,24 @@ public void execute() {
                 .withRotationalRate(
                     -rightX.getAsDouble()
                         * MaxAngularRate)); // Drive counterclockwise with negative X
-        //        System.out.println("TELEOP");
+        drivetrainState = "Teleop Drive";
         break;
 
       // (Needs check) Mode for when shots are from a still position
       case STILL_SHOT:
         // make X with wheels, set wheels to brake mode
         applyRequest.ModuleStates = states;
         subsystem.setControl(applyRequest);
-        //        System.out.println("Drivetrain: Still Shot Configuration");
+        drivetrainState = "Still Shot";
         break;
 
       // (Incomplete) Mode for moving shots
       case SOTM:
         // shoot on the move, reference Mechanical Advantage build log
-        //        System.out.println("Drivetrain: SOTM Drive");
+        drivetrainState = "Shoot on the Move Drive";
         break;
 
       case AUTO_ALIGN_HUB:
-        //        System.out.println("AUTO ALIGN");
         // Recompute the desired heading toward the hub each loop
         m_targetAngle =
             ShotCalculator.getRotationTowardsHub(
@@ -155,7 +155,7 @@ public void execute() {
           subsystem.setControl(
               drive.withVelocityX(0.0).withVelocityY(0.0).withRotationalRate(rotOutput));
         }
-
+        drivetrainState = "Auto Align";
         break;
 
       default:

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

@@ -21,7 +21,7 @@ public static enum Position {
   private Flywheel subsystem;
   private Position pose;
   private CommandSwerveDrivetrain drivetrain;
-  public static String isOn = "";
+  public static String flywheelState = "Stopped";
 
   public FlywheelCommand(Flywheel subsystem, Position pose, CommandSwerveDrivetrain drivetrain) {
     this.pose = pose;
@@ -37,34 +37,25 @@ public void execute() {
       // Spins flywheels at proper speed for shooting from hub
       case HUB_SHOT:
         subsystem.shoot(FlywheelConfig.FLYWHEEL_HUB_SHOT_SPEED);
-        isOn = "true";
-        //                 System.out.println("Flywheel: Hub Shot");
+        flywheelState = "Hub Shot";
         break;
 
       // Spins flywheels at proper speed for shooting from tower
       case TOWER_SHOT:
         subsystem.shoot(FlywheelConfig.FLYWHEEL_TOWER_SHOT_SPEED);
-        isOn = "true";
-
-        //                 System.out.println("Flywheel: Tower Shot");
+        flywheelState = "Tower Shot";
         break;
 
       case TRENCH_SHOT:
         subsystem.shoot(FlywheelConfig.FLYWHEEL_TRENCH_SHOT_SPEED);
-        isOn = "true";
-
-        //        System.out.println("Flywheel: Trench Shot");
+        flywheelState = "Trench Shot";
         break;
 
       // Spins flywheels at estimated speed given distance from hub
       case CALCULATED_SHOT:
-        isOn = "true";
+        flywheelState = "Calculated Shot";
         subsystem.shoot(
             ShotCalculator.calculateFlywheelShot(drivetrain.getState().Pose.getTranslation()));
-        System.out.println(
-            "Flywheel: Calculated Shot"
-                + ShotCalculator.calculateFlywheelShot(
-                    drivetrain.getState().Pose.getTranslation()));
         break;
 
       // Determines if robot should be prepared to shoot (if during active period or 5 seconds
@@ -74,13 +65,13 @@ public void execute() {
         subsystem.shoot(
             ShotCalculator.calculateFlywheelDefaultShot(
                 drivetrain.getState().Pose.getTranslation()));
-        //         System.out.println("Flywheel: Default Shot");
+        flywheelState = "Default Shot";
         break;
 
       // Runs flywheels in outtake direction in case of jam
       case OUTTAKE:
         subsystem.outtake(FlywheelConfig.FLYWHEEL_OUTTAKE_SPEED);
-        // System.out.println("Flywheel: Outtake");
+        flywheelState = "Outtake";
         break;
 
       default:
@@ -91,7 +82,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     subsystem.stop();
-    isOn = "false";
+    flywheelState = "none";
   }
 
   @Override

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

@@ -34,27 +34,21 @@ public void initialize() {
       // extend
       case EXTEND_RETRACT:
         if (isExtended) {
-          System.out.println("Hopper: Retracting");
           subsystem.move(HopperConfig.HOPPER_RETRACT_ROTATION);
           RobotContainer.isExtended = false;
-          System.out.println(RobotContainer.isExtended);
         } else {
-          System.out.println("Hopper: Extending");
           subsystem.move(HopperConfig.HOPPER_EXTEND_ROTATION);
           RobotContainer.isExtended = true;
-          System.out.println(RobotContainer.isExtended);
         }
         break;
 
-      // (Needs check) Retracts then extends hopper back to original position to push balls into
+      // Retracts then extends hopper back to original position to push balls into
       // kicker for shooting
       case SHOOT_RETRACT:
-        //        System.out.println("Hopper: Shoot Retracting");
         subsystem.slowMove(HopperConfig.HOPPER_RETRACT_ROTATION);
         break;
 
       case SHOOT_EXTEND:
-        //        System.out.println("Hopper: Shoot Extending");
         subsystem.move(HopperConfig.HOPPER_EXTEND_ROTATION);
         break;
 

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

@@ -29,7 +29,6 @@ public void initialize() {
       // Runs intake
       case INTAKE:
         subsystem.intake(IntakeConfig.INTAKE_INTAKE_SPEED);
-        //        System.out.println("Intake: Intaking");
         break;
 
       // Runs intake in outtaking direction
@@ -40,7 +39,6 @@ public void initialize() {
       // Slow intake for shooting
       case SLOW_INTAKE:
         subsystem.intake(IntakeConfig.INTAKE_SLOW_SPEED);
-        //        System.out.println("Intake: run for shooting");
         break;
 
       default:

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

@@ -14,6 +14,7 @@ public static enum Position {
 
   private Kicker subsystem;
   private Position pose;
+  public static String kickerState = "Stopped";
 
   public KickerCommand(Kicker subsystem, Position pose) {
     this.pose = pose;
@@ -28,11 +29,13 @@ public void execute() {
       // Intakes balls to shooter
       case INTAKE:
         subsystem.intake(KickerConfig.KICKER_INTAKE_SPEED);
+        kickerState = "Intaking";
         break;
 
       // Pushes balls out of shooter area to hopper area
       case OUTTAKE:
         subsystem.outtake(KickerConfig.KICKER_OUTTAKE_SPEED);
+        kickerState = "Outtaking";
         break;
 
       default:
@@ -43,6 +46,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     subsystem.stop();
+    kickerState = "Stopped";
   }
 
   @Override

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

@@ -14,6 +14,7 @@
 @Logged
 public class Hopper extends SubsystemBase {
   protected TalonFX hopper;
+  public static String hopperState = "None";
 
   public Hopper() {
 
@@ -52,25 +53,41 @@ public Hopper() {
 
   public void move(double rotation) {
     hopper.setControl(new MotionMagicVoltage(rotation).withSlot(0));
+    if (rotation == HopperConfig.HOPPER_EXTEND_ROTATION) {
+      hopperState = "Move: Extending";
+    } else if (rotation == HopperConfig.HOPPER_RETRACT_ROTATION) {
+      hopperState = "Move: Retracting";
+    } else {
+      hopperState = "Move: Unknown";
+    }
   }
 
   public void slowMove(double rotation) {
     hopper.setControl(new MotionMagicVoltage(rotation).withSlot(1));
+    if (rotation == HopperConfig.HOPPER_EXTEND_ROTATION) {
+      hopperState = "Slow Move: Extending";
+    } else if (rotation == HopperConfig.HOPPER_RETRACT_ROTATION) {
+      hopperState = "Slow Move: Retracting";
+    } else {
+      hopperState = "Slow Move: Unknown";
+    }
   }
 
   public void extendDirectional(double speed) {
     speed = Math.abs(speed);
     hopper.setControl(new VoltageOut(-speed));
+    hopperState = "Extend Directional";
   }
 
   public void retractDirectional(double speed) {
     speed = Math.abs(speed);
     hopper.setControl(new VoltageOut(-speed));
+    hopperState = "Retract Directional";
   }
 
   public void stop() {
     hopper.stopMotor();
-    System.out.println("Hopper: Stopped");
+    hopperState = "Stopped";
   }
 
   public void zero() {
@@ -109,23 +126,10 @@ public static double getRotation(boolean isExtended) {
   public void toggleExtend() {
     if (extended) {
       move(HopperConfig.HOPPER_RETRACT_ROTATION);
-      System.out.println("Hopper: retracting");
       extended = false;
     } else {
       move(HopperConfig.HOPPER_EXTEND_ROTATION);
-      System.out.println("Hopper: extending");
       extended = true;
     }
   }
-
-  //    @Override
-  //    public void periodic() {
-  //      if (DriverStation.isDisabled()) {
-  //        hopper.setNeutralMode(NeutralModeValue.Coast);
-  //      } else if (isRetractedByPosition()) {
-  //        hopper.setNeutralMode(NeutralModeValue.Brake);
-  //      } else {
-  //        hopper.setNeutralMode(NeutralModeValue.Coast);
-  //      }
-  //    }
 }

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

@@ -13,6 +13,7 @@
 @Logged
 public class Intake extends SubsystemBase {
   protected TalonFX intake;
+  public static String intakeState = "Stopped";
 
   public Intake() {
     intake = new TalonFX(CANMappings.INTAKE_MOTOR_ID);
@@ -34,14 +35,23 @@ public Intake() {
   public void intake(double speed) {
     speed = Math.abs(speed);
     intake.setControl(new VoltageOut(speed));
+    if (speed == IntakeConfig.INTAKE_INTAKE_SPEED) {
+      intakeState = "Intaking: Default";
+    } else if (speed == IntakeConfig.INTAKE_SLOW_SPEED) {
+      intakeState = "Intaking: Slow";
+    } else {
+      intakeState = "Intaking: Unknown";
+    }
   }
 
   public void outtake(double speed) {
     speed = Math.abs(speed);
     intake.setControl(new VoltageOut(-speed));
+    intakeState = "Outtaking";
   }
 
   public void stop() {
     intake.stopMotor();
+    intakeState = "Stopped";
   }
 }