 // Calculates the cylindrical coordinates of the wrist from the servo angles
 void calcPosition()
 {   
-   double s = g_angle_d[SHL], e = g_angle_d[ELB] - 180, w = g_angle_d[WRI] - 180;
+   double s = pulseToDeg( SHL, g_pulse[SHL] ), e = pulseToDeg( ELB, g_pulse[ELB] ) - 180, w = pulseToDeg( WRI, g_pulse[WRI] ) - 180;
    
    g_radius_mm = HUMERUS*cos( radians(s) ) + ULNA*cos( radians(s + e) ) + HAND*cos( radians(s + e + w) );
    
    g_height_mm = BASE_HEIGHT + HUMERUS*sin( radians(s) ) + ULNA*sin( radians(s + e) ) + HAND*sin( radians(s + e + w) );
    
    DEBUG("g_radius_mm: "); DEBUG(g_radius_mm); DEBUG(", g_height_mm: "); DEBUGln(g_height_mm);
+}
+
+int degToPulse ( int id, double angle_d )
+{
+  return (int) round(GRADIENT[id]*angle_d + OFFSET[id]);
+}
+
+double pulseToDeg ( int id, int pulseLength )
+{
+  return ((double)pulseLength - OFFSET[id])/GRADIENT[id];
 }
 
 /*********/

   DEBUG("slowMoveArm("); DEBUG(basAngle_d); DEBUG(","); DEBUG(shlAngle_d); DEBUG(","); DEBUG(elbAngle_d); DEBUG(","); DEBUG(wriAngle_d); DEBUGln(")");
   
   int ret_val;
   double bDiff = abs( basAngle_d - g_angle_d[BAS] );
   double sDiff = abs( shlAngle_d - g_angle_d[SHL] );
   double eDiff = abs( elbAngle_d - g_angle_d[ELB] );
   double wDiff = abs( wriAngle_d - g_angle_d[WRI] );
   
   double steps = max( bDiff, sDiff );
   steps = max( steps, eDiff );
   steps = max( steps, wDiff );
   steps = round(steps);
   DEBUG("Steps: "); DEBUGln(steps);
   
   if( steps < 2 )
   {
      ret_val = moveArm( basAngle_d, shlAngle_d, elbAngle_d, wriAngle_d );
   }
   else
   {
      double bStep = (basAngle_d - g_angle_d[BAS])/steps;
      double sStep = (shlAngle_d - g_angle_d[SHL])/steps;
      double eStep = (elbAngle_d - g_angle_d[ELB])/steps;
      double wStep = (wriAngle_d - g_angle_d[WRI])/steps;
      
      double bMove = g_angle_d[BAS] + bStep;
      double sMove = g_angle_d[SHL] + sStep;
      double eMove = g_angle_d[ELB] + eStep;
      double wMove = g_angle_d[WRI] + wStep;
      
      for(int i=0; i<steps; i++)
      {
         if( ( ret_val = moveArm( bMove, sMove, eMove, wMove ) ) != 0 ) break;
         bMove += bStep; sMove += sStep; eMove += eStep; wMove += wStep;
      }
   }

   servoWrite( ELB, ELB_PARK );
   servoWrite( WRI, WRI_PARK );
   servoWrite( GRI, GRI_OPEN );
   //DEBUG("Robot parked: ("); DEBUG(g_pulse[BAS]); DEBUG(","); DEBUG(g_pulse[SHL]); DEBUG(","); DEBUG(g_pulse[ELB]); DEBUG(","); DEBUG(g_pulse[WRI]); DEBUG(","); DEBUG(g_pulse[GRI]); DEBUGln(")");
   DEBUG("Robot parked: {"); DEBUG(pulseToDeg( BAS, g_pulse[BAS] )); DEBUG(","); DEBUG(pulseToDeg( SHL, g_pulse[SHL] )); DEBUG(","); DEBUG(pulseToDeg( ELB, g_pulse[ELB] )); DEBUG(","); DEBUG(pulseToDeg( WRI, g_pulse[WRI] )); DEBUGln(",delay}");
   calcPosition();
   DEBUG("Robot parked: <"); DEBUG(g_radius_mm); DEBUG(","); DEBUG(g_height_mm); DEBUG(","); DEBUG(pulseToDeg( BAS, g_pulse[BAS] )); DEBUG(","); DEBUG(g_hand_d); DEBUGln(",delay>");
   DEBUGln("Enter r to return robot to park position\n");
   //slide(LEFT,MAX_STEPS);
    
