Make your own free website on Tripod.com
 
Linear interpolation

Ok. Let improve operating software to do linear interpolation.

To do this - 

1) Update position in rectangular coordinate system.
2) Calculate each joint angle from updated rectangular coordinate position.
3) Then re-calculate rectangular coordinate position from joint angle.

(In below I used quick basic statement style formula)

If robot had only 3 joint, each joint angle will be fixed if arm tip's rectangular coordinate position was fixed. (right?)

J3 angle will be calculate from X-Y position.

   j(3) = ATN(py / px) / .00174533#

Then calculate Xm (projected arm length to X-Y plane)

  xm = px / COS(j(3) * .00174533#) 

angG and lngA will be -

     angG = ATN(pz / xm) 
     lngA = SQR(xm ^ 2 + pz ^ 2) 
     lngB = armlength1 
     lngC = armlength2

then we will have angA and angC from formura below -

  angA = 2 * ATN(SQR(ABS((lngA ^ 2 - (lngB - lngC) ^ 2) / ((lngB + lngC) ^ 2 - lngA ^ 2)))) 
  angC = 2 * ATN(SQR(ABS((lngC ^ 2 - (lngB - lngA) ^ 2) / ((lngA + lngB) ^ 2 - lngC ^ 2))))

J1 and J2 angle was found

  j(1) = (angC + angG) / .00174533# 
  j(2) = (angC + angG + angA) / .00174533#

4) re-calicurate rectangular coordinate position.

    pz1 = SIN(j(1) * .00174533#) * armlength1 
    pz = pz1 - SIN((1800 - j(2)) * .00174533#) * armlength2 
    xm1 = COS(j(1) * .00174533#) * armlength1 
    xm = xm1 + COS((1800 - j(2)) * .00174533#) * armlength2 
    py = SIN(j(3) * .00174533#) * xm 
    px = COS(j(3) * .00174533#) * xm
 


 
 
Home Prev Next