y2024/control_loops/superstructure/superstructure_goal.fbs

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

namespace y2024.control_loops.superstructure;

// Represents goal for the intake pivot and rollers
// INTAKE will extend the pivot and turn on the rollers to intake the note.
// SPIT will extend the pivot and turn on the rollers (in reverse) to spit out the note.
enum IntakeGoal : ubyte {
    NONE = 0,
    INTAKE = 1,
    SPIT = 2,
}

enum IntakePivotGoal : ubyte {
    UP = 0,
    DOWN = 1,
}

// Represents goal for climber
// FULL_EXTEND is for fully extending the climber
// RETRACT is for retracting the climber
enum ClimberGoal : ubyte {
    FULL_EXTEND = 0,
    RETRACT = 1,
    STOWED = 2,
}

enum AutoAimMode: ubyte {
    NONE = 0, // No auto aim.
    SPEAKER = 1, // Auto aim for the speaker shot.
    SHUTTLE = 2, // Auto aim for the shuttle shot.
}

table ShooterGoal {
    catapult_goal:frc971.control_loops.catapult.CatapultGoal (id: 0);

    auto_aim: AutoAimMode (id: 1);

    // Position for the turret when we aren't auto aiming
    turret_position: frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 2);

    // Position for the altitude when we aren't auto aiming
    altitude_position: frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 3);

    // If true, we started with the ball loaded and should proceed to that state.
    preloaded:bool = false (id: 4);
}

// Represents goal for the note movement through the robot
// to various scoring positions
// NONE represents no goal for the note
// AMP represents the goal to move the note and the extend to the AMP scoring position
// TRAP represents the goal to move the note and the extend to the TRAP scoring position
// CATAPULT represents the goal to load the note in the catapult.
// It will complete the catapult goal before accepting new goals.
enum NoteGoal : ubyte {
    NONE = 0,
    AMP = 1,
    TRAP = 2,
    CATAPULT = 3,
}


table Goal {
    intake_goal:IntakeGoal = NONE (id: 0);
    intake_pivot:IntakePivotGoal = UP (id: 5);

    // Deprecated since climber no longer has an encoder
    climber_goal:ClimberGoal (id: 1);

    // Voltage we want to give to climber
    // Positive voltage is for climber up
    // Negative voltage is for climber down
    climber_goal_voltage:double = 0.0 (id: 8);

    shooter_goal:ShooterGoal (id: 2);
    note_goal:NoteGoal (id: 3);
    fire: bool (id: 4);

    // Tells the climber to go absurdly slow on FULL_EXTEND
    // Deprecated now because we take care of this through 
    // climber_goal_voltage
    slow_climber: bool = false (id: 6, deprecated);

    // Spit on the extend motors
    spit_extend: bool = false (id: 7);
}
root_type Goal;