y2023/control_loops/superstructure/superstructure_position.fbs

include "frc971/control_loops/control_loops.fbs";

namespace y2023.control_loops.superstructure;

table ArmPosition {
  // Values of the encoder and potentiometer at the base of the proximal
  // (connected to drivebase) arm in radians.
  // Zero is upwards, positive is a forwards rotation
  proximal:frc971.PotAndAbsolutePosition (id: 0);

  // Values of the encoder and potentiometer at the base of the distal
  // (connected to proximal) arm in radians.
  // Zero is straight up, positive is a forwards rotation
  distal:frc971.PotAndAbsolutePosition (id: 1);

  // Zero for roll joint is up vertically
  // Positive position would be rotated counterclockwise relative to the robot
  roll_joint:frc971.PotAndAbsolutePosition (id: 2);
}

table CANFalcon {
  // The CAN id of the falcon
  id:int (id: 0);

  // In Amps
  supply_current:float (id: 1);

  // In Amps
  // Stator current where positive current means torque is applied in
  // the motor's forward direction as determined by its Inverted setting.
  torque_current:float (id: 2);

  // In Volts
  supply_voltage:float (id: 3);

  // In degrees Celsius
  device_temp:float (id: 4);

  // Raw position
  position:float (id: 5);

  // Nominal range is -1 to 1, but can be -2 to +2
  duty_cycle:float (id: 6);
}

table Position {
    arm:ArmPosition (id: 0);

    // Zero for wrist is facing staright outward.
    // Positive position would be upwards
    wrist:frc971.AbsolutePosition (id: 1);

    // Estimated position of a cone in the gripper from the time-of-flight
    // sensors.
    // If greater than 0.9, indicates that we cannot see a cone.
    // Will be larger when the cone is farther forwards on the robot when
    // the wrist and arm positions are all at zero (this will typically mean
    // that it is larger when the cone is to the robot's current right when
    // trying to core).
    cone_position:double (id: 2);

    // If this is true, the cube beam break is triggered.
    end_effector_cube_beam_break:bool (id: 3);

    // Roller falcon data
    roller_falcon:CANFalcon (id: 4);
}

root_type Position;