mc9s12dt256 problems with trig functions

[Franklin]

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>

[Lundin]

Sounds like stack overflow. double is either 4 or 8 bytes, they will easily chew up all the stack space for you. You have somewhere around 30 doubles in that snippet alone, that's possibly 240 bytes of the stack space, unless the compiler managed to optimize some of them away.

When you have downloaded the program in the debugger, set the whole stack area to contain some garbage value, like 0xAA. Then run the program for a while and see if there are any 0xAA left at the bottom of the stack.

[Franklin]

Setting the stack values sounds like a useful trick. But tedious. Suppose I find out that I am running out of stack space. What can I do? Would using floats instead of doubles solve my problem? Is there any way I can make the stack bigger? Can I somehow split up the calculation?

[Lundin]

Setting the stack values to 0xAA takes 5 seconds, then you run the program for a short while. How is that tedious? Keeping track of the memory used is the programmer's responsibility no matter.

You can change the stack size in the .prm file of Codewarrior.

The first thing you should do however, is to optimize the code. Like you suggest, you can probably split the calculation into several functions. Then most of the variables would only exist on the stack for a short while.

Another good optimization is to ask yourself "do I need float in this particular case?". For trig functions, yes you will need them. For simple divison etc, you don't need them. It is always a good practice to regard float numbers as a necessary evil: they are not only inefficient, but also problematic in various cases. You can't compare for equality, you need to be cautious when mixing them with integers, there is the issue with float point inaccuracy etc etc.

I don't know if changing from float to double will solve anything, it depends on whether you set double as 64 or 32 bit. This is project- and compiler-dependant. Most floating point functions in C use double anyhow, so C will convert your floats to doubles in most cases.