lady_brown.cpp

#include "taolib/taolib.h"
#include "vex.h"

vex::controller controller(vex::primary);
vex::competition competition;

// Devices - you know the drill (at least, I hope you do).
vex::rotation rotation_sensor(vex::PORT1, false);
vex::motor lady_brown_motor(vex::PORT2, vex::ratio36_1, false);

void autonomous(void) {}

// These constants are angles that should be adjusted depending
// on the orientation of your rotation sensor as well as your
// arm's resting and raised (prime-state) positions. You can
// measure this using the devices panel or by logging out sensor
// positions.
constexpr double LADY_BROWN_POSITION_LOWERED = 203.0;
constexpr double LADY_BROWN_POSITION_RAISED = 232.0;

// Initial target = lowered.
// That means we should start with the arm holding in the lowered
// position until it is explicitly changed by a button press.
double lady_brown_target = LADY_BROWN_POSITION_LOWERED;

// This is a toggle for the PID controller. When using L1 and L2, we
// want to give the driver manual control over the arm to score the
// ring, so we disable the PID controller and instead just use voltage
// control.
bool use_pid = true;

// This is our PID controller, tuned with the gains I personally use,
// but you should tune this depending on the weight/power of your arm.
// (keep in mind I use a 22W 33RPM lift)
//
// If you don't know how to tune a PID controller, check out this doc I wrote:
// https://docs.rs/evian/latest/evian/control/pid/index.html#tuning
tao::PIDController lady_brown_pid({ 0.45, 0.0, 0.001, 0.0 });

void usercontrol(void) {
    while (true) {
        // needs to score or for if the arm needs slight adjustment.
        if (controller.ButtonL1.pressing()) {
            use_pid = false;
            lady_brown_motor.spin(vex::forward, 12, vex::volt);
        } else if (controller.ButtonL2.pressing()) {
            use_pid = false;
            lady_brown_motor.spin(vex::forward, -12, vex::volt);
        } else if (!use_pid) {
            lady_brown_motor.stop(vex::hold);
        } else {
            // If we are using the PID controller (not manually controlling the arm),
            // then we use the output of the PID controller as our input voltage to
            // the motor.
            double voltage = lady_brown_pid.update(
                rotation_sensor.position(vex::degrees) - lady_brown_target,
                0.01
            );
            
            lady_brown_motor.spin(vex::forward, voltage, vex::volt);
        }

        vex::wait(10, vex::msec);
    }
}

int main() {
    // Set up callbacks for autonomous and driver control periods.
    competition.autonomous(autonomous);
    competition.drivercontrol(usercontrol);

    // EVENT: If button B is pressed, toggle between arm positions.
    //
    // LOGIC:
    // - If Button B is pressed and the arm is in its "raised" state, move
    //   the arm to its "lowered" state.
    // - If Button B is pressed and the arm is in its "lowered" state, move
    //   the arm to its "raised" state.
    // - If Button B is pressed and the driver was previously controlling the
    //   arm using L1/L2, switch back to using PID and move the arm to its
    //   "raised" state.
    controller.ButtonB.pressed([]() {
        if (!use_pid) {
            // User was controlling the arm manually, but pressed B, so we switch
            // back into raised mode.
            use_pid = true;
            lady_brown_target = LADY_BROWN_POSITION_RAISED;
        } else if (lady_brown_target == LADY_BROWN_POSITION_LOWERED) {
            // User had the arm lowered, but pressed B, so we raise it.
            lady_brown_target = LADY_BROWN_POSITION_RAISED;
        } else if (lady_brown_target == LADY_BROWN_POSITION_RAISED) {
            // User had the arm raised, but pressed B, so we lower it.
            lady_brown_target = LADY_BROWN_POSITION_LOWERED;
        }
    });

    // Prevent main from exiting with an infinite loop.
    while (true) {
        vex::wait(100, vex::msec);
    }
}