//-------------PID CONTROL------------------------------ // PID CONTROL VARIABLES DECLARATION #define TAB_SIZE 10 int16_t erreur_tab[TAB_SIZE]; uint8_t ind_ecr_erreur_tab=0; int16_t integrale_erreur=0; int16_t derivee_erreur=0; int16_t Kd=0; //de 0 à 50 int16_t Ki=0; int16_t Kp=10; uint16_t consigne_posit = 0; // variable that holds the value of the position reference uint16_t posit = 0; //variable that holds the value of the position measurement int16_t erreur = 0; // variable that calculates the error void update_tab_erreur(int16_t new_erreur) { //calcul intégrale integrale_erreur=integrale_erreur-erreur_tab[ind_ecr_erreur_tab]; integrale_erreur=integrale_erreur+new_erreur; //calcul dérivée uint8_t ind_lec_erreur_tab=ind_ecr_erreur_tab+5; if (ind_lec_erreur_tab>=TAB_SIZE ) //gestion modulo ind_lec_erreur_tab-=TAB_SIZE; derivee_erreur=new_erreur-erreur_tab[ind_lec_erreur_tab]; //maj tableau erreur_tab[ind_ecr_erreur_tab]=new_erreur; ind_ecr_erreur_tab++; if (ind_ecr_erreur_tab>=TAB_SIZE) ind_ecr_erreur_tab=0; }//end of update_tab_erreur //----------------------------------------------- void pid_correction(int16_t erreur){ int16_t commande=0; //control signal int16_t commande_max=250; //maximum control signal erreur = erreur*Kp; erreur += Kd*derivee_erreur; //quand l'erreur diminue, ce terme est négatif et permet d'éviter le dépassement erreur += (Ki*integrale_erreur)/TAB_SIZE; commande=abs(erreur); // updates the control signal if (commande>commande_max) commande=commande_max; // checks for saturation if (commande<3) { motor.run(RELEASE); motor.setSpeed(0); } else if (erreur<0) { motor.run(FORWARD); motor.setSpeed(commande); } else { motor.run(BACKWARD); motor.setSpeed(commande); } }//end of pid_correction //------------------------------------------------