y2022/control_loops/superstructure/superstructure_status.fbs

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

namespace y2022.control_loops.superstructure;

// Contains which intake has a ball
enum IntakeState : ubyte {
  NO_BALL,
  INTAKE_FRONT_BALL,
  INTAKE_BACK_BALL,
}

// State of the superstructure state machine
enum SuperstructureState : ubyte {
  // Before a ball is intaked, when neither intake beambreak is triggered
  IDLE,
  // Transferring ball with transfer rollers. Moves turret to loading position.
  TRANSFERRING,
  // Loading the ball into the catapult
  LOADING,
  // The ball is loaded into the catapult
  LOADED,
  // Waiting for the turret to be at shooting goal and then telling the
  // catapult to fire.
  SHOOTING,
}

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 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);
  // The state of the superstructure

  state:SuperstructureState (id: 10);
  // Intaking state
  intake_state:IntakeState (id: 11);
  // Whether the flippers are open for shooting
  flippers_open:bool (id: 12);
  // Whether the flippers failed to open and we are retrying
  reseating_in_catapult:bool (id: 13);
  // Whether the catapult was told to fire,
  // meaning that the turret and flippers are ready for firing
  // and we were asked to fire. Different from fire flag in goal.
  fire:bool (id: 14);

  // Subsystem statuses
  climber:frc971.control_loops.RelativeEncoderProfiledJointStatus (id: 2);

  // Estimated angles and angular velocities of the intakes.
  intake_front:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 3);
  intake_back:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 4);

  turret:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 5);

  catapult:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 6);

  solve_time:double (id: 7);
  mpc_active:bool (id: 8);

  // The number of shots we have taken.
  shot_count:int32 (id: 9);

  aimer:AimerStatus (id: 15);
}

root_type Status;