pmsm-control/test_sw/controllers.c

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