Application source code.

The main real time application file is robot.c. In this file, it can be found the two application threads, moverobot and controller. The first thread is listening from a fifo. Every time the user sends a move command, the thread generates the correct references in order to make it. The second thread runs the controller that tries to achieve references established by the first thread(moverobotthread). Let's take a look to two main functions needed to move the robot: the trajectory generator and the PID controller.

• Trajectory setting up: According to the reference type (ramp, spline,scale), a different trajectory is generated. As a example of one of the implemented controllers, the source code the PID is listed. The fuction that implements the PID recieves as parameter the position generated each time by the trajectory generator in order to make the robot move, expressed in encoder counts for each axis. First, what is done is to initialize PID parameters and period. Next, the robot's position, speed and force is read. Then the PID equation is applied. Finally the control action is sended to the motors axis as a voltage.

  /**********************************/
  /* 
    Force sensor controller.
  */
  /**********************************/
  void *controller4(void *param)  
  { 
     .....
    
    // initialize PID parameters.  
    fKp_x=0.004;
    fKp_y=0.004;
    fKp_z=0.004;
    fKd_x=0.001;
    fKd_y=0.001;
    fKd_z=0.001;
    fKi_x=0.04;
    fKi_y=0.04;
    fKi_z=0.04;
    
    fKf=0.006;
    f=0.05;
  
    pos=*((position *)param);
    
    // initializa variables.
    Posdesitjada_X=pos.posX;
    Posdesitjada_Y=pos.posY;
    Posdesitjada_Z=pos.posZ;
    
    fPos_x=readEncoder(1);
    fPos_y=readEncoder(2);
    fPos_z=readEncoder(3);
  
    fVel_x=readSpeed(1);
    fVel_y=readSpeed(2);
    fVel_z=readSpeed(3);
    fFor_z=read_fs_z();
  
    fError_x=Posdesitjada_X-fPos_x;
    fError_y=Posdesitjada_Y-fPos_y;
    fError_z=Posdesitjada_Z-fPos_z;
  
    fErrAnt_x=fError_x;
    fErrAnt_y=fError_y;
    fErrAnt_z=fError_z;
    
    if (Posdesitjada_Z>0)
      {
       fTau_x=fKp_x*fError_x-fKd_x*
              fVel_x+fKi_x*
              (((fError_x+fErrAnt_x)/2)*f);
       fTau_y=fKp_y*fError_y-fKd_y*
              fVel_y+fKi_y*
              (((fError_y+fErrAnt_y)/2)*f);
       fTau_z=fKp_z*
              fError_z-fKd_z*
              fVel_z+fKi_z*
              (((fError_z+fErrAnt_z)/2)*f);
        
        fErrAnt_x=fError_x;
        fErrAnt_y=fError_y;
        fErrAnt_z=fError_z;
        
      } 
    else 
      { 
        fTau_x=fKp_x*fError_x-fKd_x*
               fVel_x+fKi_x*
               (((fError_x+fErrAnt_x)/2)*f);
        fTau_y=fKp_y*fError_y-fKd_y*
               fVel_y+fKi_y*
               (((fError_y+fErrAnt_y)/2)*f);
        fTau_z=fKf*(Posdesitjada_Z-fFor_z);
            
        fErrAnt_x=fError_x;
        fErrAnt_y=fError_y;
        fErrAnt_z=fError_z;
  
      }
  
  // Control action.    
    writeTau(0,fTau_x);
    writeTau(1,fTau_y);
    writeTau(2,fTau_z);
   
     ..........
  
   }