First thing I'd do different is not enable EA global interrupts until AFTER I've done all the interrupt initialization. That way you are assured to have the device in a known state before any interrupts can take control. The last thing you want to happen is for an interrupt to trigger BEFORE you've had a chance to properly intialize the interrupt service routines.

I scanned through the firmware and one thing looks wrong according to assumed symmetry. The STEER_LEFT and RIGHT routines pulse a control line until a input pin goes low at which time PAUSE does one pulse on the opposite control line P1.0, while its symmetric routine BA, pulses the same control line one more time after detecting the low control line. It looks like a typical case of copying a routine from above and missing one of the distinctions.

If that isn't the problem... I'll look at the code more closely.

-Jay C. Box