 //----------------------------------------------------------------------------
 void setup()
 { //Set up pins
+      STEER_SERVO.attach(STEER_OUT_PIN);
       pinMode(BRAKE_OUT_PIN, OUTPUT);
-      pinMode(STEER_OUT_PIN, OUTPUT);
+      
       // SPI: set the slaveSelectPin as an output:
       pinMode (SelectAB, OUTPUT);
       pinMode (SelectCD, OUTPUT);

volatile unsigned long RC_elapsed[7];
volatile boolean synced = false;
volatile unsigned long last_fallingedge_time = 4294967295; // max long
volatile int RC_Done[7] = {0,0,0,0,0,0,0};

long speed_errors[ERROR_HISTORY];
long old_turn_degx1000;

  ProcessFallOfINT(RC_RVS);
  RC_Done[RC_RVS] = 1;
  attachInterrupt(IRPT_RVS, ISR_RVS_rise, RISING);
  interrupts();
}

void ISR_SWITCH_rise() {
  noInterrupts();
  ProcessRiseOfINT(RC_RDR);
  attachInterrupt(digitalPinToInterrupt(IRPT_SWITCH), ISR_SWITCH_fall, FALLING);
  //RC_Done[RC_RDR] = 1;
  interrupts();
}

void ISR_SWITCH_fall() {
  noInterrupts();
  ProcessFallOfINT(RC_RDR);
  RC_Done[RC_RDR] = 1;
  attachInterrupt(digitalPinToInterrupt(IRPT_SWITCH), ISR_SWITCH_rise, RISING);
  interrupts();
}
/*---------------------------------------------------------------------------------------*/
