y2023/control_loops/superstructure/superstructure_status.fbs

include "frc971/control_loops/control_loops.fbs";
include "y2023/vision/game_pieces.fbs";
include "frc971/control_loops/profiled_subsystem.fbs";

namespace y2023.control_loops.superstructure;

enum ArmState : ubyte {
    UNINITIALIZED = 0,
    ZEROING = 1,
    DISABLED = 2,
    GOTO_PATH = 3,
    RUNNING = 4,
    ESTOP = 5,
}

table ArmStatus {
  // State of the estimators.
  proximal_estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState (id: 0);
  distal_estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState (id: 1);
  roll_joint_estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState (id: 18);

  // The node we are currently going to.
  current_node:uint32 (id: 2);
  // Distance (in radians) to the end of the path.
  path_distance_to_go:float (id: 3);
  // Goal position and velocity (radians)
  goal_theta0:float (id: 4);
  goal_theta1:float (id: 5);
  goal_theta2:float (id: 19);
  goal_omega0:float (id: 6);
  goal_omega1:float (id: 7);
  goal_omega2:float (id: 20);

  // Current position and velocity (radians)
  theta0:float (id: 8);
  theta1:float (id: 9);
  theta2:float (id: 21);

  omega0:float (id: 10);
  omega1:float (id: 11);
  omega2:float (id: 22);

  // Estimated voltage error for the two joints.
  voltage_error0:float (id: 12);
  voltage_error1:float (id: 13);
  voltage_error2:float (id: 23);

  // Current arm position in meters for use with UI
  arm_x:float (id: 24);
  arm_y:float (id: 25);
  // Circular index to handle theta wrapping
  arm_circular_index:int (id: 26);

  // True if we are zeroed.
  zeroed:bool (id: 14);

  // True if the arm is zeroed.
  estopped:bool (id: 15);

  // The current state machine state.
  state:ArmState (id: 16);

  // The number of times the LQR solver failed.
  failed_solutions:uint32 (id: 17);
}

// State of the superstructure state machine
enum EndEffectorState : ubyte {
  // Not doing anything
  IDLE = 0,
  // Intaking the game object into the end effector
  INTAKING = 1,
  // The game object is loaded into the end effector
  LOADED = 2,
  // Waiting for the arm to be at shooting goal and then telling the
  // end effector to spit
  SPITTING = 3,
}

table Status {
  // All subsystems know their location.
  zeroed:bool (id: 0);

  // If true, we have aborted. This is the or of all subsystem estops.
  estopped:bool (id: 1);

  arm:ArmStatus (id: 2);

  wrist:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 3);

  end_effector_state:EndEffectorState (id: 4);

  game_piece:vision.Class (id: 5);

  // Indicates the current lateral position of the game piece, in meters.
  // This number will be zero when the game piece is centered, and positive if
  // the arm + wrist are all at zero and the game piece is towards the back
  // of the robot. This will typically mean that positive is to the robot's
  // left
  game_piece_position:double (id: 6);
}

root_type Status;