frc971/control_loops/drivetrain/drivetrain_status.fbs

include "frc971/control_loops/control_loops.fbs";

namespace frc971.control_loops.drivetrain;

// For logging information about what the code is doing with the shifters.
table GearLogging {
  // Which controller is being used.
  controller_index:byte;

  // Whether each loop for the drivetrain sides is the high-gear one.
  left_loop_high:bool;
  right_loop_high:bool;

  // The states of each drivetrain shifter.
  left_state:byte;
  right_state:byte;
}

// For logging information about the state of the shifters.
table CIMLogging {
  // Whether the code thinks each drivetrain side is currently in gear.
  left_in_gear:bool;
  right_in_gear:bool;

  // The angular velocities (in rad/s, positive forward) the code thinks motors
  // on each side of the drivetrain are moving at.
  left_motor_speed:double;
  right_motor_speed:double;

  // The velocity estimates for each drivetrain side of the robot (in m/s,
  // positive forward) that can be used for shifting.
  left_velocity:double;
  right_velocity:double;
}

// Logging information for the polydrivetrain implementation.
table PolyDriveLogging {
  // Calculated velocity goals for the left/right sides of the drivetrain, in
  // m/s.
  goal_left_velocity:float;
  goal_right_velocity:float;
  // Feedforward components of the left/right voltages.
  ff_left_voltage:float;
  ff_right_voltage:float;
}

enum PlanningState : byte {
  NO_PLAN,
  BUILDING_TRAJECTORY,
  PLANNING_TRAJECTORY,
  PLANNED,
}

// For logging information about the state of the trajectory planning.
table TrajectoryLogging {
  // state of planning the trajectory.
  planning_state:PlanningState;

  // State of the spline execution.
  is_executing:bool;
  // Whether we have finished the spline specified by current_spline_idx.
  is_executed:bool;

  // The handle of the goal spline.  0 means stop requested.
  goal_spline_handle:int;
  // Handle of the executing spline.  -1 means none requested.  If there was no
  // spline executing when a spline finished optimizing, it will become the
  // current spline even if we aren't ready to start yet.
  current_spline_idx:int;
  // Handle of the spline that is being optimized and staged.
  planning_spline_idx:int;

  // Expected position and velocity on the spline
  x:float;
  y:float;
  theta:float;
  left_velocity:float;
  right_velocity:float;
  distance_remaining:float;
}

// For logging state of the line follower.
table LineFollowLogging {
  // Whether we are currently freezing target choice.
  frozen:bool;
  // Whether we currently have a target.
  have_target:bool;
  // Absolute position of the current goal.
  x:float;
  y:float;
  theta:float;
  // Current lateral offset from line pointing straight out of the target.
  offset:float;
  // Current distance from the plane of the target, in meters.
  distance_to_target:float;
  // Current goal heading.
  goal_theta:float;
  // Current relative heading.
  rel_theta:float;
}

// Current states of the EKF. See hybrid_ekf.h for detailed comments.
table LocalizerState {
  // X/Y field position, in meters.
  x:float;
  y:float;
  // Current heading, in radians.
  theta:float;
  // Current estimate of the left encoder position, in meters.
  left_encoder:float;
  // Velocity of the left side of the robot.
  left_velocity:float;
  // Current estimate of the right encoder position, in meters.
  right_encoder:float;
  // Velocity of the right side of the robot.
  right_velocity:float;
  // Current "voltage error" terms, in V.
  left_voltage_error:float;
  right_voltage_error:float;
  // Estimate of the offset between the encoder readings and true rotation of
  // the robot, in rad/sec.
  angular_error:float;
  // Current difference between the estimated longitudinal velocity of the robot
  // and that experienced by the wheels, in m/s.
  longitudinal_velocity_offset:float;
  // Lateral velocity of the robot, in m/s.
  lateral_velocity:float;
}

table DownEstimatorState {
  quaternion_x:double;
  quaternion_y:double;
  quaternion_z:double;
  quaternion_w:double;

  // Side-to-side and forwards/backwards pitch numbers. Note that we do this
  // instead of standard roll/pitch/yaw euler angles because it was a pain to
  // try and numerically stable roll/pitch/yaw numbers, and Eigen's interface
  // doesn't resolve the redundancies quite how we'd like.
  // Lateral pitch is the side-to-side pitch of the robot; longitudinal pitch is
  // the forwards to backwards pitch of the robot; longitudinal_pitch
  // corresponds with the traditional usage of "pitch".
  // All angles in radians.
  lateral_pitch:float;
  longitudinal_pitch:float;
  // Current yaw angle (heading) of the robot, as estimated solely by
  // integrating the Z-axis of the gyro (in rad).
  yaw:float;

  // Current position of the robot, as determined solely from the
  // IMU/down-estimator, in meters.
  position_x:float;
  position_y:float;
  position_z:float;

  // Current velocity of the robot, as determined solely from the
  // IMU/down-estimator, in meters / sec.
  velocity_x:float;
  velocity_y:float;
  velocity_z:float;

  // Current acceleration of the robot, with pitch/roll (but not yaw)
  // compensated out, in meters / sec / sec.
  accel_x:float;
  accel_y:float;
  accel_z:float;

  // Current acceleration that we expect to see from the accelerometer, assuming
  // no acceleration other than that due to gravity, in g's.
  expected_accel_x:float;
  expected_accel_y:float;
  expected_accel_z:float;

  // Current estimate of the overall acceleration due to gravity, in g's. Should
  // generally be within ~0.003 g's of 1.0.
  gravity_magnitude:float;

  consecutive_still:int;
}

table ImuZeroerState {
  // True if we have successfully zeroed the IMU.
  zeroed:bool;
  // True if the zeroing code has observed some inconsistency in the IMU.
  faulted:bool;
  // Number of continuous zeroing measurements that we have accumulated for use
  // in the zeroing.
  number_of_zeroes:int;

  // Current zeroing values beind used for each gyro axis, in rad / sec.
  gyro_x_average:float;
  gyro_y_average:float;
  gyro_z_average:float;
}

table Status {
  // Estimated speed of the center of the robot in m/s (positive forwards).
  robot_speed:double;

  // Estimated relative position of each drivetrain side (in meters).
  estimated_left_position:double;
  estimated_right_position:double;

  // Estimated velocity of each drivetrain side (in m/s).
  estimated_left_velocity:double;
  estimated_right_velocity:double;

  // The voltage we wanted to send to each drivetrain side last cycle.
  uncapped_left_voltage:double;
  uncapped_right_voltage:double;

  // The voltage error for the left and right sides.
  left_voltage_error:double;
  right_voltage_error:double;

  // The profiled goal states.
  profiled_left_position_goal:double;
  profiled_right_position_goal:double;
  profiled_left_velocity_goal:double;
  profiled_right_velocity_goal:double;

  // The KF offset
  estimated_angular_velocity_error:double;
  // The KF estimated heading.
  estimated_heading:double;

  // xytheta of the robot.
  x:double;
  y:double;
  theta:double;

  // True if the output voltage was capped last cycle.
  output_was_capped:bool;

  // The pitch of the robot relative to the ground--only includes
  // forwards/backwards rotation.
  ground_angle:double;

  // Information about shifting logic and curent gear, for logging purposes
  gear_logging:GearLogging;
  cim_logging:CIMLogging;

  trajectory_logging:TrajectoryLogging;

  line_follow_logging:LineFollowLogging;

  poly_drive_logging:PolyDriveLogging;

  down_estimator:DownEstimatorState;

  localizer:LocalizerState;

  zeroing:ImuZeroerState;
}

root_type Status;