mc_control.c

Go to the documentation of this file.

//******************************************************************************
//******************************************************************************

/* Copyright (c) 2008, Atmel Corporation All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * 3. The name of ATMEL may not be used to endorse or promote products derived
 * from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY ATMEL ``AS IS'' AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY AND
 * SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */


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

#include "dac\dac_drv.h"

U8 duty_cycle = 0;    
U8 regulation_type = OPEN_LOOP; 
U8 duty_cycle_reference = 0; 
U8 duty_cycle_filter_presc = 0;

/* 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;

#define K_DUTY 1

/************************************************************************************************************/
/*                                  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;
*/
  speed_integral = Ki_speed*(speed_integral + speed_error);

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

  // 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_reference = mci_get_ref_speed();break;
    case SPEED_LOOP    : duty_cycle_reference = mc_control_speed(2*mci_get_ref_speed());break;
    case CURRENT_LOOP  : duty_cycle_reference = 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;
}

void duty_cycle_filter (void)
{
  duty_cycle_filter_presc++;
  if   (duty_cycle_filter_presc == 30)
  {
    duty_cycle_filter_presc = 0;
  if (duty_cycle < duty_cycle_reference)
  {
    duty_cycle = duty_cycle + K_DUTY;
  }
  else
  {
    if (duty_cycle > duty_cycle_reference)
    {
      duty_cycle = duty_cycle - K_DUTY;
    }
  }
  }
}

---