mc_control.c

Go to the documentation of this file.

#include "config.h"
#include "config_motor.h"

#include "mc_control.h"
#include "mc_drv.h"
#include "mc_interface.h"

#include "mc_interface.h"

U8 duty_cycle = 0;    
U8 BO_BF = SPEED_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;

/* Position regulation variables */
S16 pos_error = 0;      
S16 last_pos_error = 0;  
S16 pos_integral = 0;
S16 pos_integ = 0;
S16 pos_proportional = 0;
S16 pos_derivative = 0;
S16 pos_der = 0;

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

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

  // proportional term calculation
  speed_proportional = (S16)Kp_speed * speed_error;

  // integral term calculation
  speed_integral = speed_integral + speed_error;

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

  speed_integ = (S16)Ki_speed * speed_integral;

  // derivative term calculation
/*  speed_derivative = (S16)speed_error - last_speed_error;

  if(speed_derivative >  (S16)255) speed_derivative =  255;
  if(speed_derivative < (S16)-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;
  increment = increment + Klin_speed;

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

  /* return the new "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;
}

/************************************************************************************************************/
/*                                  Position Regulation                                                     */
/************************************************************************************************************/
U8 mc_control_position(S32 Number_of_turns)
{
  U8 Duty = 0;
  S32 increment = 0;

  // Error calculation
  pos_error =  Number_of_turns - mc_get_Num_Turn();

  // proportional term calculation
  pos_proportional = Kp_pos*pos_error;

  // integral term calculation
  pos_integral = pos_integral + pos_error;

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

  pos_integ = Ki_pos * pos_integral;

  // derivative term calculation
  pos_derivative = pos_error - last_pos_error;

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

  pos_der = Kd_pos*pos_derivative;

  last_pos_error = pos_error;

  // Duty Cycle calculation
  increment = pos_proportional + pos_integ;
  increment += pos_der;
  increment = increment >> K_pos_scal;

  // Variable saturation
  if(increment >= (S16)(0))
  {
    if(increment >= (S16)(100))Duty = 100;
    else Duty = (U8)(increment);
    mci_set_motor_direction(CW);
  }
  else
  {
    if(increment <= (S16)(-100)) Duty = 100;
    else Duty = (U8)(-increment);
    mci_set_motor_direction(CCW);
  }
  // return Duty Cycle
  return Duty;
}


/************************************************************************************************************/
/*                                Selection of Regulation Loop                                              */
/************************************************************************************************************/
void mc_regulation_loop()
{
  // measure
  Set_EXT3();

  switch(BO_BF)
  {
    case OPEN_LOOP     : duty_cycle = mci_get_motor_speed();break;
    case SPEED_LOOP    : duty_cycle = mc_control_speed(mci_get_motor_speed());break;
    case CURRENT_LOOP  : duty_cycle = mc_control_current(mci_get_potentiometer_value());break;
    case POSITION_LOOP : duty_cycle = /*mc_control_speed(*/mc_control_position(mci_get_potentiometer_value())/*)*/;break;
    default : break;
  }

  // measure
  Clear_EXT3();
}

void mc_set_Open_Loop(){BO_BF = OPEN_LOOP;}

void mc_set_Speed_Loop(){BO_BF = SPEED_LOOP;}

void mc_set_Current_Loop(){BO_BF = CURRENT_LOOP;}

void mc_set_Position_Loop(){BO_BF = POSITION_LOOP;}

U8 mc_get_Duty_Cycle()
{
  return duty_cycle;
}

---