[Bongle]

We got around this very effectively this year by taking a test-driven approach.

Split whatever code you're trying to test out into its own class, and have its entire interface with the robot be a single function call. Make sure the controlling code doesn't talk directly with WPILib (or else it ups the complexity of doing your simulated tests) We had something like:

class ArmController
{
public:
  Tick(double currentTime, double armPosition, double* shoulderPower, double* elbowPower);
private:
  PIDController m_myPidController;
  int myCurrentState; // 0 = stowing, 1 = going out, 2 = out, 3 = stowed, etc.
};

Usage in the robot code looks like:

double shoulderPower=0;
double elbowPower=0;
m_armController.Tick(currentTime, m_armPot.Get(), &shoulderPower, &elbowPower);

m_shoulderMotor.set(shoulderPower);
m_elbowMotor.set(elbowPower);

Then, elsewhere (in our case in Visual Studio), write some other code like:

ArmController controller;
double desiredShoulderPower;
double desiredElbowPower;
controller.Tick(0, OUT_POSITION, &desiredShoulderPower, &desiredElbowPower);

assert(desiredShoulderPower == 0); // the arm is out, so we shouldn't be trying to drive it
assert(desiredElbowPower == 0); // the arm is out, so we shouldn't be trying to drive it.

And do that for every other state that the arm might be in. We ended up with 20-30 unique tests for our arm controller, found a _ton_ of subtle bugs (some that we may not have found while doing in-pit testing anyway), and it worked _flawlessly_. We wrote it in the stands at Waterloo while the team was unbagging the robot, and basically didn't even give it a classic on-robot test.