Florentin von Frankenberg

mc9s12dt256 problems with trig functions

Discussion created by Florentin von Frankenberg on Mar 22, 2009
Latest reply on Mar 26, 2009 by Daniel Lundin

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>

Outcomes