 #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;

        pinMode(LED7, OUTPUT);
        pinMode(LED8, OUTPUT);  
        Serial.begin(9600);
        initialize();
        attachInterrupt(0, CruiseRC1Pressed, RISING);     
        attachInterrupt(1, StopRC2Pressed, RISING);
        attachInterrupt(3, EStopPressed, FALLING);
        attachInterrupt(4, StopPressed, RISING);
        attachInterrupt(5, CruisePressed, RISING);     
#else
        initialize();
        attachInterrupt(0, StopPressed, FALLING);
        attachInterrupt(1, CruisePressed, FALLING);     
#endif
}	

/*---------------------------------------------------------------------------------------*/ 
void initialize()
{
        Display (0xFF);  // LED test
        digitalWrite( StopLED, HIGH);
        digitalWrite( CruiseLED, HIGH);
        
        Halt();
        

  Instrument[Brakes].Enabled = digitalRead(EnableBrake);
  Instrument[Steering].Enabled = digitalRead(EnableSteer);
#ifdef TEST_MODE
    ServoData();
   
#else  // not TEST_MODE
#endif  // TEST_MODE 
 // TODO: Check out loop speed;
 // make loop faster
