   results[RC_ESTP] = (results[RC_ESTP] > MIDDLE ? HIGH : LOW);
 
   if (results[RC_ESTP] == HIGH){
+    Serial.println("Exiting processRC due to E-stop.");
     E_Stop();  // already done at interrupt level
     if ((results[RC_AUTO] == LOW)  && (NUMBER_CHANNELS > 5)) // under RC control
       ;//steer(results[RC_TURN]);
-    Serial.println("Exiting processRC due to E-stop.");
     return 0x00;
   }
 

  }
  else if(doRoutine(results[RC_RDR])){
    moveVehicle(MIN_ACC_OUT);
    squareRoutine(20, results[RC_AUTO]);
  }
  else {
    moveVehicle(MIN_ACC_OUT);

  while (millis() < (loopTime + seconds)) {
    moveVehicle(128);
  }
  rcAuto = LOW;
}
/*---------------------------------------------------------------------------------------*/
//squareRoutine
void squareRoutine(unsigned long sides, unsigned long &rcAuto) {
  Serial.println("Starting square routine...");
  rcAuto = HIGH;
  long straightSpeed = 2750;        //mmPs
  long turnSpeed = 1400;            //mmPs
  sides = sides * 1000;             //convert side length to mm
  unsigned long sideSec = sides / straightSpeed;  //calculate seconds per side at set speed
  sideSec = sideSec * 1000;         //convert to ms
  unsigned long loopTime;           //start time of while loops for throttle
  float turnDist = TURN_RADIUS_CM * PI / 2 * 10; //turnDist == 1/4 turn circumference in mm
  unsigned long turnSec = (long)turnDist / turnSpeed * 1000; //turnSec == time for 90-degree turn at 1250 mmPs
  for(int i = 0; i < 4; i++){
    //steer(LEFT_TURN_OUT);
    steer(STRAIGHT_TURN_OUT);
