    //if (angle_d < SERVO_MIN_D[id]) { DEBUG(angle_d); DEBUG("<"); DEBUGln(SERVO_MIN_D[id]); return -1; }
    //if (angle_d > SERVO_MAX_D[id]) { DEBUG(angle_d); DEBUG(">"); DEBUGln(SERVO_MAX_D[id]); return -2; }
    
-   double gradient = (SERVO_MAX[id]-SERVO_MIN[id])/(SERVO_MAX_D[id]-SERVO_MIN_D[id]);
-   double pulseLength = angle_d*gradient + SERVO_MIN[id] - gradient*SERVO_MIN_D[id];// + map(angle_d, SERVO_MIN_D[id], SERVO_MAX_D[id], SERVO_MIN[id], SERVO_MAX[id]);
-   
-   int ret_val = servoWritePulse( id, pulseLength );
+   int ret_val;
+   
+   if ( id == 4 ) ret_val = servoWritePulse( id, (int) round(angle_d) );
+   else
+   {
+     int pulseLength = round(GRADIENT[id]*angle_d + OFFSET[id]);
+     ret_val = servoWritePulse( id, pulseLength );
+   }
    
    if (ret_val != 0 ) return ret_val;
    

}

// Write to servo using pulse length
int servoWritePulse( int id, double pulseLength)
{
  if (pulseLength < min(SERVO_MIN[id], SERVO_MAX[id])) { DEBUGln(min(SERVO_MIN[id], SERVO_MAX[id])); return -1; }
  if (pulseLength > max(SERVO_MIN[id], SERVO_MAX[id])) { DEBUGln(max(SERVO_MIN[id], SERVO_MAX[id])); return -2; }
  
  pwm.setPWM(id, 0, pulseLength);
  

#define BAS_PARK 90
#define SHL_PARK 90
#define ELB_PARK 180
#define WRI_PARK 90


/*****************/
