Hi,
we are experiencing problems performing motion programs. We have implemented the kinematics of a SCARA robot. We have checked both forward and inverse kinematics in order to know they match. The 4 motors are configurated as phantom motors the same way this thread http://forums.deltatau.com/showthread.php?tid=193&highlight=phantom+motor
Jogging motors works ok but motion programs disable InPos for a second and then return true without moving then and of course not being in the desired position.
Here is the code:
Forward Kinematics
open forward (1) // Put Coordinate System number inside "(cs)"
// --------------------User Code Goes Here------------------------
paramZ(0) = 2,0,0,2,0
paramX(0) = 0,3,2,0,0
angleHome(0)= KinPosMotor1,KinPosMotor2,KinPosMotor3,KinPosMotor4;
DH(0)= 0,paramZ(0),0,0,angleHome(0),0,paramX(1),0,angleHome(1),0,paramX(2),0,0,-paramZ(3),0,0,angleHome(2),angleHome(3),0,0;
fT01(0) = cosd(DH(0)), -cosd(DH(3))*sind(DH(0)), sind(DH(3))*sind(DH(0)), DH(2)*cosd(DH(0)), sind(DH(0)), cosd(DH(3))*cosd(DH(0)), -sind(DH(3))*cosd(DH(0)), DH(2)*sind(DH(0)), 0, sind(DH(3)), cosd(DH(3)), DH(1), 0, 0, 0, 1;
fT12(0) = cosd(DH(4)), -cosd(DH(7))*sind(DH(4)), sind(DH(7))*sind(DH(4)), DH(6)*cosd(DH(4)), sind(DH(4)), cosd(DH(7))*cosd(DH(4)), -sind(DH(7))*cosd(DH(4)), DH(6)*sind(DH(4)), 0, sind(DH(7)), cosd(DH(7)), DH(5), 0, 0, 0, 1;
fT23(0) = cosd(DH(8)), -cosd(DH(11))*sind(DH(8)), sind(DH(11))*sind(DH(8)), DH(10)*cosd(DH(8)), sind(DH(8)), cosd(DH(11))*cosd(DH(8)), -sind(DH(11))*cosd(DH(8)), DH(10)*sind(DH(8)), 0, sind(DH(11)), cosd(DH(11)), DH(9), 0, 0, 0, 1;
fT34(0) = cosd(DH(12)), -cosd(DH(15))*sind(DH(12)), sind(DH(15))*sind(DH(12)), DH(14)*cosd(DH(12)), sind(DH(12)), cosd(DH(15))*cosd(DH(12)), -sind(DH(15))*cosd(DH(12)), DH(14)*sind(DH(12)), 0, sind(DH(15)), cosd(DH(15)), DH(13), 0, 0, 0, 1;
fT45(0) = cosd(DH(16)), -cosd(DH(19))*sind(DH(16)), sind(DH(19))*sind(DH(16)), DH(18)*cosd(DH(16)), sind(DH(16)), cosd(DH(19))*cosd(DH(16)), -sind(DH(19))*cosd(DH(16)), DH(18)*sind(DH(16)), 0, sind(DH(19)), cosd(DH(19)), DH(17), 0, 0, 0, 1;
MxMult = mmul(&fT02(0),&fT01(0),&fT12(0),4,4,4)
MxMult = mmul(&fT03(0),&fT02(0),&fT23(0),4,4,4)
MxMult = mmul(&fT04(0),&fT03(0),&fT34(0),4,4,4)
MxMult = mmul(&fT05(0),&fT04(0),&fT45(0),4,4,4)
KinPosAxisX = fT05(3);
KinPosAxisY = fT05(7);
KinPosAxisZ = fT05(11);
KinAxisUsed=$1C0
close
Inverse Kinematics
open inverse (1) // Put Coordinate System number inside "(cs)"
// --------------------User Code Goes Here------------------------
sA1 = (fT02(0) * fT01(1)) + (fT02(4) * fT01(5)) + (fT02(8) * fT01(9));
cA1 = (fT02(1) * fT01(1)) + (fT02(5) * fT01(5)) + (fT02(9) * fT01(9));
qA1 = atan2d(sA1,cA1);
KinPosMotor1 = qA1;
sA2 = (fT03(0) * fT02(1)) + (fT03(4) * fT02(5)) + (fT03(8) * fT02(9));
cA2 = (fT03(1) * fT02(1)) + (fT03(5) * fT02(5)) + (fT03(9) * fT02(9));
qA2 = atan2d(sA2,cA2);
KinPosMotor2 = qA2;
sA3 = (fT05(0) * fT04(1)) + (fT05(4) * fT04(5)) + (fT05(8) * fT04(9));
cA3 = (fT05(1) * fT04(1)) + (fT05(5) * fT04(5)) + (fT05(9) * fT04(9));
qA3 = atan2d(sA3,cA3);
KinPosMotor3 = qA3;
qA4 = fT05(11) - fT04(11);
KinPosMotor4 = qA4;
close
Motion Program
open prog 1
// --------------------User Code Goes Here------------------------
pmatch;
inc;
rapid;
Coord[1].SegMoveTime = 5; // Segmentation time of 5 msec
X2.63 Y4.05 Z-2 //A-90 B-87 C-90
close
/****************************************/
Any suggestions?
Thanks
