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 = 0,
  // Transferring ball with transfer rollers. Moves turret to loading position.
  TRANSFERRING = 1,
  // Loading the ball into the catapult
  LOADING = 2,
  // The ball is loaded into the catapult
  LOADED = 3,
  // Waiting for the turret to be at shooting goal and then telling the
  // catapult to fire.
  SHOOTING = 4,
}

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 front/rear intakes currently are holding balls.
  front_intake_has_ball:bool (id: 18);
  back_intake_has_ball:bool (id: 19);
  // Whether we just shot a ball and are transitioning a second one
  transitioning_second_ball:bool (id: 24);
  // 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 turret/catapult is collided with the intake
  collided:bool(id: 23);
  // Whether the turret is ready for firing
  ready_to_fire:bool (id: 20);
  // Whether the robot is moving too fast to shoot
  moving_too_fast:bool (id: 21);
  // True if the robot has detected that it is holding
  // the wrong color ball and is now discarding it.
  discarding_ball:bool (id: 22);
  // 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_horizon:uint8 (id: 8);
  shot_position:double (id: 16);
  shot_velocity:double (id: 17);

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

  aimer:AimerStatus (id: 15);
}

root_type Status;