Hi,
I am trying to use my microcontroller to control a 3 degree of freedom robot arm. I run into problems when I try to compute the position of the end-effector. I believe it is because of the trig functions I'm using in my equations. I get all sorts of strange behavior. Variables that are not supposed to change are shown as changing in my debug window, sometimes I get illegal breakpoint error. I don't know what's going on. Can anyone advise me? The code that is causing problems is shown below.
<code>void ForwardDisp(void)
{
double Px1;
double Px2;
double Px3;
double Py1;
double Py2;
double Py3;
theta1 = encoders[0]*0.000177853;
theta2 = encoders[1]*0.000177853;
theta3 = encoders[2]*0.000177853;
s1 = sin(theta1);
s2 = sin(theta2);
s3 = sin(theta3);
c1 = cos(theta1);
c2 = cos(theta2);
c3 = cos(theta3);
c23 = cos(theta2+theta3);
s23 = sin(theta2+theta3);
Px1 = (c1)*(c23)*(L2);
Px2 = (c1)*(c2)*(L1);
Px3 = (s1)*(Offset1 + Offset2);
Px = Px1+Px2+Px3;
Py1 = (s1)*(c23)*(L2);
Py2 = (s1)*(c2)*(L1);
Py3 = (c1)*(Offset1 + Offset2);
Py = Py1+Py2-Py3;
Pz = (s23)*(L2)+(s2)*(L1);
// Pz = (sin(theta2 + theta3))*(L2); //we know this worked!
}</code>