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