asservissement robot 2, C++, doc fournie
Dependencies: mbed QEI L298 Asservissement
main.cpp
- Committer:
- hamaint
- Date:
- 2019-03-19
- Revision:
- 1:11cbd2bf65d7
- Parent:
- 0:a6cbfa280472
- Child:
- 2:41e56a06c580
File content as of revision 1:11cbd2bf65d7:
#include "mbed.h" #include "QEI.h" #include "Moteur.h" #include "math.h" Serial pc(SERIAL_TX,SERIAL_RX); QEI encodeur_a (D2, D3, NC, 1200, QEI::X4_ENCODING); QEI encodeur_b (D4, D5, NC, 1200, QEI::X4_ENCODING); /*pour 8v*/ static float Pl = 0.01; //0.03 static float Il = 0.0001; //0.001 static float Dl = 0; static float Pa = 0.01; //0.01 static float Ia = 0.0001; //0.0001 static float Da = 0; #define LINEIQUE 0 #define ANGULAIRE 1 #define FREQ_CORRECTION 0.01 #define ERR_STATIQUE 100 #define LIMITE_TEMPS 1/FREQ_CORRECTION /*3 secondes*/ #define DISTANCE_ENTRE_ROUES 270 /*en mm*/ #define DIMENSION_ROUE_A 70 /*en mm*/ #define DIMENSION_ROUE_B 70 /*en mm*/ #define TIC_PAR_TOUR_A 1200 #define TIC_PAR_TOUR_B 1200 #define LIMITE_VITESSE 0.5 void pos(int commande_a, int commande_b); void pos_a(int commande_a); void pos_b(int commande_b); void deplacement(int theta, int r); float PID(float erreur,float type); int main() { float distance = 0; float orientation_deg = 90; float pulse_a; float pulse_b; float pid_a; float pid_b; float pid_dist; float pid_ori; float err_dist; float err_ori; float n_commande_a; float n_commande_b; float res_a = (float)DIMENSION_ROUE_A*3.14/(float)TIC_PAR_TOUR_A; float res_b = (float)DIMENSION_ROUE_B*3.14/(float)TIC_PAR_TOUR_B; float orientation_dist = orientation_deg * (float)DISTANCE_ENTRE_ROUES * 3.14 / 360; printf("%f\n\r",orientation_dist); moteur_init(); while(1) { pulse_a = (float)encodeur_a.getPulses() * res_a; pulse_b = (float)encodeur_b.getPulses()* res_b; err_dist = distance - (pulse_a + pulse_b) /2; err_ori = orientation_dist - (pulse_a - pulse_b) /2; printf("%f\t%f\t%f\t%f\n\r",err_dist,err_ori,pulse_a,pulse_b); pid_dist = PID(err_dist,LINEIQUE); pid_ori = PID(err_ori,ANGULAIRE); pid_a = - pid_dist + pid_ori; pid_b = - pid_dist - pid_ori; //printf("%f\n\r",pulse_a); n_commande_a = pid_a; n_commande_b = pid_b; if (n_commande_a>1) n_commande_a=LIMITE_VITESSE; else if (n_commande_a<-LIMITE_VITESSE) n_commande_a = -LIMITE_VITESSE; if (n_commande_b>LIMITE_VITESSE) n_commande_b=LIMITE_VITESSE; else if (n_commande_b<-LIMITE_VITESSE) n_commande_b = -LIMITE_VITESSE; moteur_a(n_commande_a); moteur_b(n_commande_b); wait(FREQ_CORRECTION); } return 0; } float PID(float erreur,float type) { static float errPrev = 0; static float errSum = 0; static float errDif = 0; float P,I,D; errSum += erreur; errDif = erreur - errPrev; errPrev = erreur; if (type){ P = erreur * Pl; I = errSum * Il; D = errDif * Dl; } else { P = erreur * Pa; I = errSum * Ia; D = errDif * Da; } return P + I + D; //Le résultat est la somme des trois //composantes calculées précédemment } /*asservissement des deux roues individuellement*/ /*pas très utile pour le déplacement mais intéressant*/ /*de garder comme un exemple*/ /* void pos(int commande_a, int commande_b) { int erreur_a = 0; int somme_erreur_a = 0; int derniere_erreur_a = 0; int derivee_a = 0; float n_commande_a; int validation_position_a = 0; int erreur_b = 0; int somme_erreur_b = 0; int derniere_erreur_b = 0; int derivee_b = 0; float n_commande_b; int validation_position_b = 0; do { erreur_a = encodeur_a.getPulses()-commande_a; erreur_b = encodeur_b.getPulses()-commande_b; somme_erreur_a = somme_erreur_a + erreur_a; somme_erreur_b = somme_erreur_b + erreur_b; derivee_a = erreur_a - derniere_erreur_a; derivee_b = erreur_b - derniere_erreur_b; n_commande_a = erreur_a * P + somme_erreur_a * I + derivee_a * D; n_commande_b = erreur_b * P + somme_erreur_b * I + derivee_b * D; derniere_erreur_a = erreur_a; derniere_erreur_b = erreur_b; if (n_commande_a>1) n_commande_a=1; else if (n_commande_a<-1) n_commande_a = -1; if (n_commande_b>1) n_commande_b=1; else if (n_commande_b<-1) n_commande_b = -1; moteur_a(-n_commande_a); moteur_b(-n_commande_b); if (abs(erreur_a)<ERR_STATIQUE) validation_position_a++; else validation_position_a = 0; if (abs(erreur_b)<ERR_STATIQUE) validation_position_b++; else validation_position_b = 0; wait(FREQ_CORRECTION); } while (validation_position_a<LIMITE_TEMPS || validation_position_b<LIMITE_TEMPS); moteur_a(0); moteur_b(0); encodeur_a.reset(); encodeur_b.reset(); } */