traject planner

["Luc Vercruysse" <loeka-at-mail.yucom.be>]

Hi ,

 

I am developping an "intelligent servo driver"...(I try it). It is a driver  with a microcontroller on board (89S8252 from atmel) and a ispLSI 1016 (LATTICE) that contains a quadrature decoder + counter, a pulse width modulator that feeds a H-bridge to let turn the motor.

I want to acces the board with my parallel port in EPP or ECP mode (to speed up data transfert and to acces more than one device).

The microcontroller receives the goal positions  from the EMC-interpolator, reads its actual position of the encoder, and generates a PWM signal (that feeds the H-bridge => motor) using the PID stuf from EMC (using fixed point math).

Right now I am making a prototype.

To test the microcontroller software, I separated the trajectory planner and interpolator.

Following program is suposed to move the X and Y axes to make 2 lines.

I capture positions and speed in a file and plot it out using GNUplot.

The postions seems to be OK, but when I plot the speed, there is a "spike" (speed increments up to 150% of its programmed speed) on the point where the Y-axis starts turning.

Is this normal , or is there a bug in my test program ??

 

 

PS. As soon the prototype is finished, I'll drop it in the drop box.

 

Luc Vercruysse.

 

 

void main(void)
{

 

 FILE *data;
 CUBIC_STRUCT xCubic;
 CUBIC_STRUCT yCubic;
 CUBIC_STRUCT zCubic;
 PID_STRUCT xPid;
 PID_STRUCT yPid;
 PID_STRUCT zPid;
 PID_STRUCT Pid;

 

 double OutPut[3];

 

 TP_STRUCT queue;
 TC_STRUCT tcSpace[100];

 

 PmPose pos;

 

 PmCartesian center;
 PmCartesian normal;
 PmPose end;

 

 double vMax = 0.1;
 double aMax = 1.0;
 double CycleTime = 0.01;
 double vMaxAxes = 5.0;
 double SpeedX, SpeedY, SpeedZ;

 

 data = fopen("Data.dat","wt");

 

 

 

 tpCreate(&queue, 100, tcSpace);

 

// set traj parameters

 

 tpSetCycleTime(&queue, CycleTime);
 tpSetVmax(&queue, vMax);
 tpSetAmax(&queue, aMax);
 tpSetVlimit(&queue, vMaxAxes);

 

 cubicInit(&xCubic);
 cubicSetSegmentTime(&xCubic,CycleTime / 10);
 cubicSetInterpolationRate(&xCubic,10);
 cubicInit(&yCubic);
 cubicSetSegmentTime(&yCubic,CycleTime / 10);
 cubicSetInterpolationRate(&yCubic,10);
 cubicInit(&zCubic);
 cubicSetSegmentTime(&zCubic,CycleTime / 10);
 cubicSetInterpolationRate(&zCubic,10);

 

 

 

 pos.rot.x =  0.0;
 pos.rot.y =  0.0;
 pos.rot.z =  0.0;

 

 pos.tran.x = 1.0;
 pos.tran.y = 0.0;
 pos.tran.z = 0.0;
 tpAddLine(&queue,pos);

 

 pos.tran.x = 2.0;
 pos.tran.y = 1.0;
 pos.tran.z = 0.0;
 tpAddLine(&queue,pos);

 

 while(!tpIsDone(&queue)) {

 

   while(cubicNeedNextPoint(&xCubic)) {
     tpRunCycle(&queue);
     pos = tpGetPos(&queue);
     cubicAddPoint(&xCubic,pos.tran.x);
     cubicAddPoint(&yCubic,pos.tran.y);
     cubicAddPoint(&zCubic,pos.tran.z);
   }

 

   pos.tran.x = cubicInterpolate(&xCubic,0,&SpeedX,0,0);
   pos.tran.y = cubicInterpolate(&yCubic,0,&SpeedY,0,0);
   pos.tran.z = cubicInterpolate(&zCubic,0,&SpeedZ,0,0);
   fprintf(data,"\n%f %f %f %f",pos.tran.x,
    pos.tran.y,
    pos.tran.z,
    sqrt(SpeedX*SpeedX + SpeedY*SpeedY + SpeedZ*SpeedZ));
 } 
 fclose(data);
}