mc_control.c

Go to the documentation of this file.

#ifdef __ICCAVR__ // IAR C Compiler
#include "config.h"
#include "inavr.h"
#endif

#ifdef __GNUC__  // GNU C Compiler
#include "config_for_gcc.h"
#endif


/* Speed control variables */
S16 speed_error=0;           
S16 speed_integral = 0;      
S16 speed_integ = 0;         
S16 speed_proportional = 0;  

/**************************************************************************************/
/*                          Speed Control                                             */
/**************************************************************************************/
U16 mc_control_speed_16b(U16 speed_ref , U16 speed_measure)
{
   U16 result = 0;
   S16 increment = 0;

   // Error calculation
//   speed_error = speed_ref - speed_measure/2 ;
   speed_error = speed_ref - speed_measure;

   // proportional term calculation : Kp= 7/64=0.1
   speed_proportional = speed_error * 4;

   // integral term calculation
   speed_integral = speed_integral + speed_error;

   // speed integral saturation
   if(speed_integral >  32000) speed_integral =  32000;
   if(speed_integral < -32000) speed_integral = -32000;

   speed_integ = (speed_integral - speed_integral/8 + speed_integral/32) / 16 ;

   // amplitude calculation
   increment = speed_proportional + speed_integ;

   // saturation of the PI output
   if (increment > 0)
   {
      if  (increment <= (S16)(MAX_AMPLITUDE)) result = (S16)increment ;
      else result = MAX_AMPLITUDE ;
   }
   else result = 0;

   // return Duty Cycle
   return result;
}

---