mc_control.c

Go to the documentation of this file.

//_____  I N C L U D E S ___________________________________________________
#include "config.h"
#include "mc_control.h"
#include "mc_drv.h"
#include "mc_interface.h"

U8 duty_cycle = 0;    
U8 regulation_type = OPEN_LOOP; 

/* Speed regulation variables */
S16 speed_error = 0;      
S16 last_speed_error = 0;  
S16 speed_integral = 0;
S16 speed_integ = 0;
S16 speed_proportional = 0;
S16 speed_derivative = 0;
S16 speed_der = 0;

/* Current regulation variables */
S16 cur_error = 0;      
S16 last_cur_error = 0;  
S16 cur_integral = 0;
S16 cur_integ = 0;
S16 cur_proportional = 0;
S16 cur_derivative = 0;
S16 cur_der = 0;


/************************************************************************************************************/
/*                                  Speed Regulation                                                        */
/************************************************************************************************************/
U8 mc_control_speed(U8 speed_cmd)
{
  U8 Duty = 0;
  S32 increment = 0;

  // Error calculation
  speed_error = speed_cmd - mci_get_measured_speed();// value -255 <=> 255

  // proportional term calculation
  speed_proportional = Kp_speed*speed_error;

  // integral term calculation
  speed_integral = speed_integral + speed_error;

  if(speed_integral >  255) speed_integral =  255;
  if(speed_integral < -255) speed_integral = -255;

  speed_integ = Ki_speed*speed_integral;

  // derivative term calculation
  /*speed_derivative = speed_error - last_speed_error;

  if(speed_derivative >  255) speed_derivative =  255;
  if(speed_derivative < -255) speed_derivative = -255;

  speed_der = Kd_speed*speed_derivative;

  last_speed_error = speed_error;*/

  // Duty Cycle calculation
  increment = speed_proportional + speed_integ;
  //increment += speed_der;
  increment = increment >> K_speed_scal;

  // Variable saturation
  if(increment >= (S16)(255)) Duty = 255;
  else
  {
    if(increment <= (S16)(0)) Duty =   0;
    else Duty = (U8)(increment);
  }

  // return Duty Cycle
  return Duty;
}

/************************************************************************************************************/
/*                                  Current Regulation                                                      */
/************************************************************************************************************/
U8 mc_control_current(U8 cur_cmd)
{
  U8 Duty = 0;
  S32 increment = 0;

  // Error calculation
  cur_error = cur_cmd - (mci_get_measured_current());// value -255 <=> 255

  // proportional term calculation
  cur_proportional = Kp_cur*cur_error;

  // integral term calculation
  cur_integral = cur_integral + cur_error;

  if(cur_integral >  255) cur_integral =  255;
  if(cur_integral < -255) cur_integral = -255;

  cur_integ = Ki_cur*cur_integral;

  // derivative term calculation
  /*cur_derivative = cur_error - last_cur_error;

  if(cur_derivative >  255) cur_derivative =  255;
  if(cur_derivative < -255) cur_derivative = -255;

  cur_der = Kd_cur*cur_derivative;

  last_cur_error = cur_error;*/

  // Duty Cycle calculation
  increment = cur_proportional + cur_integ;
  //increment += cur_der;
  increment = increment >> K_cur_scal;

  // Variable saturation
  if(increment >= (S16)(255)) Duty = 255;
  else
  {
    if(increment <= (S16)(0)) Duty =   0;
    else Duty = (U8)(increment);
  }

  // return Duty Cycle
  return Duty;
}



/************************************************************************************************************/
/*                                Selection of Regulation Loop                                              */
/************************************************************************************************************/
void mc_regulation_loop()
{
   switch(regulation_type)
  {
    case OPEN_LOOP     : duty_cycle = mc_get_motor_speed();break;
    case SPEED_LOOP    : duty_cycle = mc_control_speed(mc_get_motor_speed());break;
    case CURRENT_LOOP  : duty_cycle = mc_control_current(mc_get_potentiometer_value());break;
    default : break;
  }
}

void mc_set_Open_Loop(){regulation_type = OPEN_LOOP;}

void mc_set_Speed_Loop(){regulation_type = SPEED_LOOP;}

void mc_set_Current_Loop(){regulation_type = CURRENT_LOOP;}


U8 mc_get_Duty_Cycle()
{
  return duty_cycle;
}