y2024/control_loops/superstructure/superstructure_status.fbs

include "frc971/control_loops/control_loops.fbs";
include "frc971/control_loops/profiled_subsystem.fbs";

namespace y2024.control_loops.superstructure;

// Contains if intake is intaking
enum IntakeRollerStatus : ubyte {
  NONE = 0,
  SPITTING = 1,
  INTAKING = 2,
}

enum CatapultState: ubyte {
    // Means we are waiting for a game piece
    READY = 0,
    // Means we have a game piece
    LOADED = 1,
    // Means we are firing a game piece
    FIRING = 2,
    // We are retracting the bucket to do it again.
    RETRACTING = 3,
}

table AimerStatus {
  // The current goal angle for the turret auto-tracking, in radians.
  turret_position:double (id: 0);
  // The current goal velocity for the turret, in radians / sec.
  turret_velocity:double (id: 1);
  // The current distance to the target, in meters.
  target_distance:double (id: 2);
  // The current "shot distance." When shooting on the fly, this may be
  // different from the static distance to the target.
  shot_distance:double (id: 3);
}

table ShooterStatus {
  // Estimated angle and angular velocitiy of the turret.
  turret:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 0);

  // Estimated angle and angular velocitiy of the catapult.
  catapult:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 1);

  // Estimated angle and angular velocitiy of the altitude.
  altitude:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 2);

  catapult_state: CatapultState (id: 3);

  // Status of the aimer
  aimer:AimerStatus (id: 4);
}

// Contains status of transfer rollers
enum TransferRollerStatus : ubyte {
    NONE = 0,
    TRANSFERING_IN = 1,
    TRANSFERING_OUT = 2,
}

// Contains status of extend rollers
// HAS_GAME_PIECE means we have a game piece
// EMPTY means we don't have a game piece
enum ExtendRollerStatus: ubyte {
  HAS_GAME_PIECE = 0,
  EMPTY = 1
}

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);

  // Status of the rollers
  intake_roller:IntakeRollerStatus (id: 2);

  // Estimated angle and angular velocitiy of the intake.
  intake_pivot:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 3);

  // Status of transfer rollers
  transfer_roller:TransferRollerStatus (id: 4);

  // Estimated angle and angular velocitiy of the climber.
  climber:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 5);

  // Status of the subsytems involved in the shooter
  shooter:ShooterStatus (id: 6);

  // Estimated angle and angular velocitiy of the extend.
  extend:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 7);

  // State of the extender rollers
  extend_roller:ExtendRollerStatus (id: 8);

  // The status of if the turret and intake is colliding
  collided: bool (id: 9);
}

root_type Status;