[kenfox]

Originally Posted by Ether

a) use a state machine

I agree with trying a state machine first.

Hand-coded state machines are a bit tedious. If you have more than a couple places you need them, the command-based robot approach works better. Robot builder will generate the basic code for your robot after you describe the subsystems and commands.

Here's an example of what a hand-coded state machine looks like.

// declare the timeout variables at the top of your Robot.cpp
static double waitForwardTimeout = 0.0;
static double waitReverseTimeout = 0.0;

...

if (waitForwardTimeout > 0.0) {
    if (Timer::GetPPCTimestamp() >= waitForwardTimeout) {
        waitForwardTimeout = 0.0;
        // DO SOMETHING AFTER WAITING FOR FORWARD TIMEOUT
        Forward->Set(0);
        Reverse->Set(1);
        waitReverseTimeout = Timer::GetPPCTimestamp + 1.5;
    }
}
else if (waitReverseTimeout > 0.0) {
    if (Timer::GetPPCTimestamp() >= waitReverseTimeout) {
        waitReverseTimeout = 0.0;
        // DO SOMETHING AFTER WAITING FOR REVERSE TIMEOUT
    }
}
else if (LeftBumper && RightBumper) {  // xBox controller
    front->Set(-speed); // Talons
    back->Set(speed);
    if (ButtonA) {
        Forward->Set(1); // solenoids
        Reverse->Set(0);
        waitForwardTimeout = Timer::GetPPCTimestamp + 0.1;
    }
}

There are many ways to implement a state machine, but that code is probably the easiest to fit into your existing code.

In case you can't recognize your code in that example, here's your code with a couple comments added to match the comments in my example:

if ((LeftBumper)&&(RightBumper)) { // xBox controller
    front->Set(-speed); // Talons
    back-> Set(speed);
    if (ButtonA) {
        Forward->Set(1); // solenoids
        Reverse->Set(0);
        Wait(0.1);
        // DO SOMETHING AFTER WAITING FOR FORWARD TIMEOUT
        Forward->Set(0);
        Reverse->Set(1);
        Wait(1.5);
        // DO SOMETHING AFTER WAITING FOR REVERSE TIMEOUT
    }
}