y2020/control_loops/superstructure/superstructure_status.fbs

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

namespace y2020.control_loops.superstructure;

enum Subsystem:byte {
  FINISHER,
  ACCELERATOR,
  TURRET
}

table FlywheelControllerStatus {
  // The current average velocity in radians/second over the last kHistoryLength
  // in shooter.h
  avg_angular_velocity:double (id: 0);

  // The current instantaneous filtered velocity in radians/second.
  angular_velocity:double (id: 1);

  // The target speed selected by the lookup table or from manual override
  // Can be compared to velocity to determine if ready.
  angular_velocity_goal:double (id: 2);

  // Voltage error.
  voltage_error:double (id: 3);

  // The commanded battery current.
  commanded_current:double (id: 4);

  // The angular velocity of the flywheel computed using delta x / delta t
  dt_angular_velocity:double (id: 5);
}

table ShooterStatus {
  // The final wheel shooting the ball
  finisher:FlywheelControllerStatus (id: 0);

  // The subsystem to accelerate the ball before the finisher
  // Velocity is the fastest (top) wheel
  accelerator_left:FlywheelControllerStatus (id: 1);
  accelerator_right:FlywheelControllerStatus (id: 2);

  // If the shooter is ready, this is true.  Note: don't use this for status
  // checking, only for plotting.  Changes in goal take time to impact this.
  ready:bool (id: 3);

  // The total number of balls we have shot
  balls_shot:int (id: 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);
  // Whether we are currently aiming for the inner port.
  aiming_for_inner_port:bool (id: 2);
  // The current distance to the target, in meters.
  target_distance:double (id: 3);
  // The current "shot distance." When shooting on the fly, this may be
  // different from the static distance to the target.
  shot_distance:double (id: 4);
}

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

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

  // Subsystem status.
  hood:frc971.control_loops.AbsoluteAndAbsoluteEncoderProfiledJointStatus (id: 2);
  intake:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 3);
  turret:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 4);

  // Shooter subsystem status.
  shooter:ShooterStatus (id: 5);

  // Status of the control_panel
  control_panel:frc971.control_loops.RelativeEncoderProfiledJointStatus (id: 6);

  // Status of the vision auto-tracking.
  aimer:AimerStatus (id: 7);

  // Vector of the subsystems that are not at goal and are preventing shooting.
  subsystems_not_ready:[Subsystem] (id: 8);
}

root_type Status;