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;

enum SuperstructureState : ubyte {
  // Before a note has been intaked, the extend should be retracted.
  IDLE = 0,
  // Intaking a note and transferring it to the extned through the
  // intake, transfer, and extend rollers.
  INTAKING = 1,
  // The note is in the extend and the extend is not moving.
  LOADED = 2,
  // The note is in the extend and the extend is moving towards a goal,
  // either the catapult, amp, or trap.
  MOVING = 3,
  // For Catapult Path, the note is being transferred between the extend and the catapult.
  LOADING_CATAPULT = 4,
  // The note is either:
  // 1. Loaded in the catapult and ready to fire
  // 2. In the extend and the extend is at the amp or trap scoring position.
  READY = 5,
  // Fire goal recieved and the note is being fired from the catapult or being scored in the amp or trap.
  FIRING = 6,
}

// 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);
  // Estimate of the retention roller current.
  note_current:double (id: 4);
  // Turret offset applied due to retention roller current.
  current_turret_offset:double (id: 5);
}

// Enum representing where the superstructure
// is currently trying to send the note.
enum NoteStatus : ubyte {
  NONE = 0,
  CATAPULT = 1,
  AMP = 2,
  TRAP = 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);

  // True if auto-aiming.
  auto_aiming:bool (id: 5);

  turret_in_range:bool (id: 6);
  altitude_in_range:bool (id: 7);
  altitude_above_min_angle:bool (id: 8);
}

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

// Contains status of extend rollers
enum ExtendRollerStatus: ubyte {
  // Means the rollers are not moving.
  IDLE = 0,
  // Means we're transfer from the transfer rollers into the extend rollers.
  TRANSFERING_TO_EXTEND = 1,
  // Means we are transfering from the extend to the catapult.
  TRANSFERING_TO_CATAPULT = 2,
  // Means we're trying to score in the amp/trap
  SCORING_IN_AMP = 3,
  SCORING_IN_TRAP = 4,
}

// Contains the status of the extend subsystem
enum ExtendStatus : ubyte  {
  // Means we are near 0 and ready to transfer a game piece.
  RETRACTED = 0,
  // Means we are moving to some goal.
  MOVING = 1,
  // Means we are currently at the catapult.
  CATAPULT = 2,
  // Means we are at the amp position.
  AMP = 3,
  // Means we are at the trap positon.
  TRAP = 4,
}

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

  state : SuperstructureState (id: 2);

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

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

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

  // Estimated angle and angular velocitiy of the climber.
  // Deprecated now because climber no longer has an encoder
  // and climber status is not used
  climber:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 6, deprecated);

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

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

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

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

  extend_status:ExtendStatus (id: 11);

  // Indicates that the extend is in position to allow a game piece to
  // be transfered into it.
  extend_ready_for_transfer:bool (id: 12);

  // Indicates that the turret is in position to avoid the extend.
  turret_ready_for_extend_move:bool (id: 13, deprecated);

  uncompleted_note_goal:NoteStatus = NONE (id: 14);

  // Indicates if the extend is close enough to the retracted position to be
  // considered ready to accept note from the transfer rollers.
  extend_at_retracted:bool (id: 15);

  // Indicates if the turret is at the position to accept the note from extend
  turret_ready_for_load:bool (id: 16);

  // Indicates if the altitude is at the position to accept the note from
  // extend
  altitude_ready_for_load:bool (id: 17);

  // Indicates if the extend is at the position to load the catapult
  extend_ready_for_catapult_transfer:bool (id: 18);

  extend_beambreak:bool (id: 19);
  catapult_beambreak:bool (id: 20);
  transfer_beambreak:bool (id: 22);

  // Number of shots we have taken.
  shot_count:uint32 (id:21);
}

root_type Status;