 {  
    int ret_val = 0;
    
+   checkPosition();
+   //if ( ( ret_val = checkPosition() ) != 0 ) return ret_val;
+   
    DEBUG("moveArm("); DEBUG(basPulse); DEBUG(","); DEBUG(shlPulse); DEBUG(","); DEBUG(elbPulse); DEBUG(","); DEBUG(wriPulse); DEBUGln(")");
    
    //Drive servos


void delayFunc ( long int delay_ )
{
  long int start = micros();
  int delay_ms = delay_/1000;
  int delay_us = delay_%1000;
  if ( delay_ms > 0 ) delay(delay_ms);
  if ( delay_us > 0 ) delayMicroseconds(delay_us);
  long int fin = micros();
  /*Serial.print(delay_ms);
  Serial.print("+");
  Serial.print(delay_us);

   double uln_h = ULN_H_GRADIENT*g_uln_h.mean() + ULN_H_OFFSET;
   double uln_angle_d = degrees(  atan2(uln_h,uln_r) );
   DEBUG("uln_r: "); DEBUG(uln_r); DEBUG(", uln_h: "); DEBUG(uln_h); DEBUG(", uln_angle_d: "); DEBUGln(uln_angle_d);
   Serial.print((double)millis()/1000.0f); Serial.print("\t"); Serial.print(uln_angle_d); Serial.print("\t"); Serial.println(s + e);
   return ( abs(uln_angle_d - (s + e)) > FB_THRESHOLD ) ? -101 : 0;
}

