pmsm-control/test_sw/controllers.c

   1
   2 #include "controllers.h"
   3
   4 /**
   5  * \brief No regulation.
   6  */
   7 inline void zero_controller(struct rpi_state* state){
   8         return;
   9 }
  10
  11 /*
  12  * \brief
  13  * Very simple PID regulator.
  14  * Now only with P-part so that the error doesnt go to zero.
  15  * TODO: add anti-wind up and I and D parts
  16  */
  17 void pos_pid(struct rpi_state* state){
  18         int duty_tmp;
  19         duty_tmp = PID_P*(state->desired_pos - (int32_t)state->spi_dat->pozice);
  20         if (duty_tmp>MAX_DUTY){
  21                 state->duty=MAX_DUTY;
  22         }else if (duty_tmp<-MAX_DUTY){
  23                 state->duty=-MAX_DUTY;
  24         }else{
  25                 state->duty = duty_tmp;
  26         }
  27 }
  28
  29 /*
  30  * \brief
  31  * Very simple PID regulator.
  32  * FIXME: make better
  33  */
  34 void spd_pid(struct rpi_state* state){
  35         int duty_tmp;
  36         int error;
  37         error=state->desired_spd - state->speed;
  38         state->spd_err_sum+=error;
  39         duty_tmp = PID_P_S*error+PID_I_S*state->spd_err_sum;
  40         if (duty_tmp>MAX_DUTY){
  41                 state->duty=MAX_DUTY;
  42         }else if (duty_tmp<-MAX_DUTY){
  43                 state->duty=-MAX_DUTY;
  44         }else{
  45                 state->duty = duty_tmp;
  46         }
  47 }