   Instrument[Brakes].Enabled = digitalRead(EnableBrake);
   Instrument[Steering].Enabled = digitalRead(EnableSteer);
 #ifdef TEST_MODE
-//  testSwitches();
-//  testRamp();
-//    testQuick();
-//    RampTest();
     ServoData();
    
 #else  // not TEST_MODE
- 
-  JoystickMotion();
-  StateTransition(Motor);
-  StateTransition(Brakes);
-  StateTransition(Steering);
-  
-  if (Instrument[Motor].State == ManualGo)
-  {
-   Instrument[Motor].LastCommand = Instrument[Motor].Joystick;
-  }
-  else if (Instrument[Motor].State == CruiseGo)
-  {
-    Instrument[Motor].LastCommand = analogRead(CruiseThrottle) / 4;   
-  }
-  else
-  {
-    Instrument[Motor].LastCommand = MinimumThrottle;   
-  }
-   Instrument[Motor].move(Instrument[Motor].LastCommand);
-  
-  if (Instrument[Brakes].State == ManualGo)
-  {
-   Instrument[Brakes].LastCommand = Instrument[Brakes].Joystick;
-  }  
-  else if (Instrument[Brakes].State == CruiseGo)
-  {
-    Instrument[Brakes].LastCommand = analogRead(CruiseBrake) / 4;   
-  }
-  else
-  {
-    Instrument[Brakes].LastCommand = FullBrake;   
-  }
-   Instrument[Brakes].move(Instrument[Brakes].LastCommand);
-  
-   if (Instrument[Steering].State == ManualGo)
-  {
-   Instrument[Steering].LastCommand = Instrument[Steering].Joystick;
-  }  
-  else if (Instrument[Steering].State == CruiseGo)
-  {
-    Instrument[Steering].LastCommand = analogRead(CruiseSteer) / 4;   
-  }
-  Instrument[Steering].move(Instrument[Steering].LastCommand);
-
-   if (Stop)
-     digitalWrite(StopLED, HIGH);
-   else
-     digitalWrite(StopLED, LOW);
-   if (TimeNow > TimeOfCruiseButton_ms + 400 &&
-   (Instrument[Motor].StickMoved || Instrument[Brakes].StickMoved || Instrument[Steering].StickMoved))
-       Cruise = FALSE;
-   if (!Instrument[Motor].Enabled && !Instrument[Motor].Enabled && !Instrument[Motor].Enabled)
-       Cruise = FALSE;
-   if (Cruise)
-     digitalWrite(CruiseLED, HIGH);
-   else
-     digitalWrite(CruiseLED, LOW);
-
-  checkReverse();  
 #endif  // TEST_MODE 
  // TODO: Check out loop speed;
  // make loop faster

#include "IO_2009.h"
#endif

void Display (int n);
/*

There are four principle states. "Enable" is considered a generic state that encompasses the
three switches for Throttle, Brake and Steer. The four main states have additional substates
corresponding to the state of these three systems. The system can simultaneaous be in mixed states,
e.g. "Cruise Go" on Throttle and Brakes and "Manual Go" on Steering.
When Cruise is enabled on a system, "Joystick Motion" means major motion in that axis:
Left to right for Steer, Forward for Throttle, Reverse for Brake. Minor joystick motions will
not take an enabled system into manual mode, but major motion will.

MS: Manual Stop State:  Stop LED On, Cruise LED Off,
  Throttle Off, Brakes Off,  Steer responds to joystick but not to computer.
  Joystick is centered.
  Enable Off.

CS: Cruise Stop State:  Stop LED On, Cruise LED On,
  Throttle Off, Brakes Off,  Steer responds to computer.
  Joystick is centered.
  Enable On.
  
MG: Manual Go State:  Stop LED Off, Cruise LED Off,
  Throttle, Brakes, Steer respond to joystick but not to computer.

CG:Cruisel Go State:  Stop LED Off, Cruise LED On,
  Throttle, Brakes, Steer respond to computer.
  Joystick is centered.
  Enable On.

There are five controls that can transition between states:
Cruise button.
  MS -> MS; CS -> CG; MG -> CG; CG -> CG.
Joystick motion
  MS -> MG; CS -> MG; MG -> MG; CG -> MG.
Enable Switch On
  MS -> CS; CS N/A; MG -> MG; CG N/A.
Enable Switch Off
  MS N/A; CS -> MS; MG -> MG; CG -> MG.
Stop button
  MS -> MS; CS -> CS; MG -> MS; CG -> CS.
E-stop
  MS -> MS; CS -> CS; MG -> MS; CG -> CS. E-stop also removes power to the traction motor.
If the E-stop button has been pushed, transition to MG or CG is blocked until
power is restored. Restoring power is done by turning the key or pressing 
the remote E-stop button (RC4) a second time. There is a dead zone of several seconds between the
first button push and the second push, allowing the vehicle to come to a stop.

States:  */
  const int ManualStop = 0;
  const int CruiseStop = 1;
  const int ManualGo = 3;


} 
/*---------------------------------------------------------------------------------------*/ 

/*********************************************************** 
                        TEST ROUTINES
 ***********************************************************/
 
/*---------------------------------------------------------------------------------------*/ 
#ifdef TEST_MODE

//========================================================================
void BrakeWrite(int i, int Delay_ms)
{
     analogWrite(DiskBrake, i);
     for (int d = 0; d < Delay_ms; d++)
     {
       int feedback = analogRead(FeedbackBrake);
//       int current =  analogRead(CurrentBrake);
       delay(1);
       Serial.print(d); Serial.print(", ");
       Serial.print(i); Serial.print(", ");
       Serial.print(feedback); Serial.print(", ");
