Add commands to the Swerve subsystem

[jkleiber]

Summary We need some commands in the Swerve subsystem in order to use it in teleop/auto. It would be most beneficial if we create public functions in the subsystem that are used by commands to set the action we want the subsystem to take. Then in Periodic we can use some private functions as appropriate to execute the desired commands.

Here's an example of the framework for a generic subsystem:

// Note: this is public in the header file
bool Subsystem::SetPosition(const double &position) {
    this.position_ = position;
    return true;
}

void Subsystem::Periodic() { 
    // The subsystem does different things based on what mode it is put in by the user.
    switch(mode) {
        case Manual:
            OKC_CALL(SetUserPower());
            break;
        case AutoPosition: 
           OKC_CALL(GoToPosition());
            break;
        default:
             break;
    }
}

// Note: these are private in the header file:
bool Subsystem::SetUserPower() {
    // OKC_CHECKs as needed

    // Set the user power
    interface_.power = power;

    return true;
}

bool Subsystem::GoToPosition() {
    // OKC_CHECKs as needed

    // Control the position using PID

    return true;
}

Work

• Make a list of commands for the Swerve drive, and put them in a reply in this ticket
• Implement the framework described above for the Swerve subsystem
• Add commands in the commands folder that interact with the public functions created in the framework

Verification

• Commands list is acceptable
• Framework implemented correctly
• Commands are implemented correctly
• CI passes

[danielbrownmsm]

Yeah we have the commands this an be closed