7Robot_Freescale / Mbed 2 deprecated freescal_cup_k22f

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed freescal_cup_k22f

Dependents:   freescal_cup_k22f

source/Gestion_Moteur.cpp@5:4d1a524433ca, 2015-01-13 (annotated)

Committer:
RobinN7
Date:
Tue Jan 13 16:35:04 2015 +0000
Revision:
5:4d1a524433ca
Parent:
0:3af30bfbc3e5
Child:
6:a4e49784b533
popopo;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobinN7 0:3af30bfbc3e5 1 //Bibliothéque
RobinN7 0:3af30bfbc3e5 2 #include "QEI.h"
RobinN7 0:3af30bfbc3e5 3 #include "mbed.h"
RobinN7 0:3af30bfbc3e5 4 #include "Gestion_Moteur.h"
RobinN7 0:3af30bfbc3e5 5 #include "Servo.h"
RobinN7 0:3af30bfbc3e5 6
RobinN7 0:3af30bfbc3e5 7 DigitalOut led_red(LED_RED);
RobinN7 0:3af30bfbc3e5 8 DigitalOut led_green(LED_GREEN);
RobinN7 0:3af30bfbc3e5 9 //DigitalOut FLAG(PTA5);
RobinN7 0:3af30bfbc3e5 10
RobinN7 0:3af30bfbc3e5 11
RobinN7 0:3af30bfbc3e5 12 AnalogOut FLAG(DAC0_OUT);
RobinN7 0:3af30bfbc3e5 13 Timeout timeout;
RobinN7 0:3af30bfbc3e5 14 PwmOut Pwm_moteur_1(PTD4); // Moteur gauche
RobinN7 0:3af30bfbc3e5 15 PwmOut Pwm_moteur_2(PTA1); // Moteur droit
RobinN7 0:3af30bfbc3e5 16 Ticker ticker_asserv;
RobinN7 0:3af30bfbc3e5 17 Ticker ticker_servo;
RobinN7 0:3af30bfbc3e5 18 // Declaration des differents objets, variables necessaires au bon fonctionnement de la regulation des moteurs dediés à la propulsion
RobinN7 0:3af30bfbc3e5 19 double Periode_echantillonage=0.0001;
RobinN7 0:3af30bfbc3e5 20 // QEI moteur gauche (QEI pinA, QEI pinB, NC, pulses/tour)
RobinN7 0:3af30bfbc3e5 21 QEI decodeur_1 (PTB0,PTB1, NC, 30);
RobinN7 0:3af30bfbc3e5 22 // QEI moteur droit (QEI pinA, QEI pinB, NC, pulses/tour)
RobinN7 0:3af30bfbc3e5 23 QEI decodeur_2 (PTB18,PTB19, NC, 30);
RobinN7 0:3af30bfbc3e5 24
RobinN7 0:3af30bfbc3e5 25
RobinN7 0:3af30bfbc3e5 26 // Variables d'asserv moteur gauche
RobinN7 0:3af30bfbc3e5 27 // Mesure brute QEI 1, Mesure brute QEI 1 précédente, Écart mesuré, Écart de consigne
RobinN7 5:4d1a524433ca 28 int mesure1=0,mesure_precedente1=0,mesure_moteur_1,consigne_moteur_1=6;
RobinN7 0:3af30bfbc3e5 29 // Coeff proportionnel, Intégral, Valeur intégrale, valeur sortie de l'asserv, consigne PWM envoyée au moteur gauche
RobinN7 0:3af30bfbc3e5 30 float Kp_moteur_1=2,Ki_moteur_1=2,I_x_moteur_1=0,sortie_asserv1,consigne_PWM1;
RobinN7 0:3af30bfbc3e5 31 // Variables d'asserv moteur droit
RobinN7 5:4d1a524433ca 32 int mesure2=0,mesure_precedente2=0,mesure_moteur_2,consigne_moteur_2=6;
RobinN7 0:3af30bfbc3e5 33 // Coeff proportionnel, Intégral, Valeur intégrale, valeur sortie de l'asserv, consigne PWM envoyée au moteur droit
RobinN7 0:3af30bfbc3e5 34 float Kp_moteur_2=2,Ki_moteur_2=2,I_x_moteur_2=0,sortie_asserv2,consigne_PWM2;
RobinN7 0:3af30bfbc3e5 35
RobinN7 0:3af30bfbc3e5 36 // Pins de sortie PWM
RobinN7 0:3af30bfbc3e5 37
RobinN7 0:3af30bfbc3e5 38
RobinN7 0:3af30bfbc3e5 39 double i=0.4;
RobinN7 0:3af30bfbc3e5 40 /*
RobinN7 0:3af30bfbc3e5 41 void move_servo()
RobinN7 0:3af30bfbc3e5 42 {
RobinN7 0:3af30bfbc3e5 43
RobinN7 0:3af30bfbc3e5 44 float mem;
RobinN7 0:3af30bfbc3e5 45 mem=FLAG.read();
RobinN7 0:3af30bfbc3e5 46 FLAG.write(0.5);
RobinN7 0:3af30bfbc3e5 47
RobinN7 0:3af30bfbc3e5 48 if (i<0.7) i+=0.001;
RobinN7 0:3af30bfbc3e5 49 else i=0.4;
RobinN7 0:3af30bfbc3e5 50 servo = i;
RobinN7 0:3af30bfbc3e5 51
RobinN7 0:3af30bfbc3e5 52 FLAG.write(mem);
RobinN7 0:3af30bfbc3e5 53 }
RobinN7 0:3af30bfbc3e5 54 */
RobinN7 0:3af30bfbc3e5 55 void mesure_interruption(void)
RobinN7 0:3af30bfbc3e5 56 {
RobinN7 0:3af30bfbc3e5 57 float mem;
RobinN7 0:3af30bfbc3e5 58 mem=FLAG.read();
RobinN7 0:3af30bfbc3e5 59 FLAG.write(1);
RobinN7 0:3af30bfbc3e5 60 // Calcul de l'écart pour le moteur gauche
RobinN7 0:3af30bfbc3e5 61 mesure1=decodeur_1.getPulses();
RobinN7 0:3af30bfbc3e5 62 if (mesure1 >= mesure_precedente1)
RobinN7 0:3af30bfbc3e5 63 mesure_moteur_1=mesure1-mesure_precedente1;
RobinN7 0:3af30bfbc3e5 64 else
RobinN7 0:3af30bfbc3e5 65 mesure_moteur_1=mesure1-mesure_precedente1+32767;
RobinN7 0:3af30bfbc3e5 66 mesure_precedente1=mesure1;
RobinN7 0:3af30bfbc3e5 67
RobinN7 0:3af30bfbc3e5 68 // Calcul de l'écart pour le moteur droit
RobinN7 0:3af30bfbc3e5 69 mesure2=decodeur_2.getPulses();
RobinN7 0:3af30bfbc3e5 70 if (mesure2 >= mesure_precedente2)
RobinN7 0:3af30bfbc3e5 71 mesure_moteur_2=mesure2-mesure_precedente2;
RobinN7 0:3af30bfbc3e5 72 else
RobinN7 0:3af30bfbc3e5 73 mesure_moteur_2=mesure2-mesure_precedente2+32767;
RobinN7 0:3af30bfbc3e5 74 mesure_precedente2=mesure2;
RobinN7 0:3af30bfbc3e5 75
RobinN7 0:3af30bfbc3e5 76
RobinN7 0:3af30bfbc3e5 77 Bloc_propulsion();
RobinN7 0:3af30bfbc3e5 78 FLAG.write(mem);
RobinN7 0:3af30bfbc3e5 79 }
RobinN7 0:3af30bfbc3e5 80 void init_led(void)
RobinN7 0:3af30bfbc3e5 81 {
RobinN7 0:3af30bfbc3e5 82 // Ticker asserv périodique :
RobinN7 0:3af30bfbc3e5 83 //timeout.attach_us(&retard_servo,10);
RobinN7 0:3af30bfbc3e5 84 //ticker_servo.attach_us(&move_servo,0.05*1000000);
RobinN7 0:3af30bfbc3e5 85 ticker_asserv.attach_us(&mesure_interruption,Periode_echantillonage*1000000);
RobinN7 0:3af30bfbc3e5 86 led_red=1;
RobinN7 0:3af30bfbc3e5 87 led_green=1;
RobinN7 0:3af30bfbc3e5 88 Pwm_moteur_1.period_us(100);
RobinN7 0:3af30bfbc3e5 89 Pwm_moteur_2.period_us(100);
RobinN7 0:3af30bfbc3e5 90 //
RobinN7 0:3af30bfbc3e5 91 }
RobinN7 0:3af30bfbc3e5 92
RobinN7 0:3af30bfbc3e5 93
RobinN7 0:3af30bfbc3e5 94 double Regulateur_PI_decodeur(double consigne,double mesure, double Kp, double Ki ,double Periode_echantillonage,double I_x )
RobinN7 0:3af30bfbc3e5 95 {
RobinN7 0:3af30bfbc3e5 96 // Kp Ki sont des parametres Globals modifiable via la transmission série
RobinN7 0:3af30bfbc3e5 97 // Periode_echantillonage est une constante à tous le système
RobinN7 0:3af30bfbc3e5 98 // I_x est une valeur interne du regulateur
RobinN7 0:3af30bfbc3e5 99 // la consigne est la vitesse calculé par le calculateur moteur
RobinN7 0:3af30bfbc3e5 100 // la mesure est la vitesse mesuré par les qei
RobinN7 0:3af30bfbc3e5 101
RobinN7 0:3af30bfbc3e5 102 //Consigne et Mesure doivent être dans les mêm unité : rad/s
RobinN7 0:3af30bfbc3e5 103 double ecart, P_x,commande;
RobinN7 0:3af30bfbc3e5 104 /////// Régulation PI ///////
RobinN7 0:3af30bfbc3e5 105 // Ecart entre la consigne et la mesure
RobinN7 0:3af30bfbc3e5 106 ecart = consigne - mesure;
RobinN7 0:3af30bfbc3e5 107
RobinN7 0:3af30bfbc3e5 108 // Terme proportionnel
RobinN7 0:3af30bfbc3e5 109 P_x = Kp * ecart;
RobinN7 0:3af30bfbc3e5 110
RobinN7 0:3af30bfbc3e5 111 // Calcul de la commande
RobinN7 0:3af30bfbc3e5 112 commande = P_x + I_x;
RobinN7 0:3af30bfbc3e5 113
RobinN7 0:3af30bfbc3e5 114 // Terme intégral (sera utilisé lors du pas d'échantillonnage suivant)
RobinN7 0:3af30bfbc3e5 115 I_x = I_x + Ki * Periode_echantillonage * ecart;
RobinN7 0:3af30bfbc3e5 116 /////// Fin régulation PID ///////
RobinN7 0:3af30bfbc3e5 117 return commande;
RobinN7 0:3af30bfbc3e5 118 }
RobinN7 0:3af30bfbc3e5 119
RobinN7 0:3af30bfbc3e5 120 void Bloc_propulsion()
RobinN7 0:3af30bfbc3e5 121 {
RobinN7 0:3af30bfbc3e5 122 //
RobinN7 0:3af30bfbc3e5 123 sortie_asserv1 = (float)Regulateur_PI_decodeur(consigne_moteur_1, mesure_moteur_1, Kp_moteur_1, Ki_moteur_1, Periode_echantillonage,I_x_moteur_1 );
RobinN7 0:3af30bfbc3e5 124 //
RobinN7 0:3af30bfbc3e5 125 sortie_asserv2 = (float)Regulateur_PI_decodeur(consigne_moteur_2, mesure_moteur_2, Kp_moteur_2, Ki_moteur_2, Periode_echantillonage,I_x_moteur_2 );
RobinN7 0:3af30bfbc3e5 126
RobinN7 0:3af30bfbc3e5 127 //Conversion en PWM
RobinN7 0:3af30bfbc3e5 128 consigne_PWM1=sortie_asserv1/20;
RobinN7 0:3af30bfbc3e5 129 consigne_PWM2=sortie_asserv2/20;
RobinN7 0:3af30bfbc3e5 130
RobinN7 0:3af30bfbc3e5 131 // Saturation
RobinN7 0:3af30bfbc3e5 132 // Moteur gauche
RobinN7 0:3af30bfbc3e5 133 if (consigne_PWM1 > 1)
RobinN7 0:3af30bfbc3e5 134 consigne_PWM1=1;
RobinN7 0:3af30bfbc3e5 135 else if (consigne_PWM1 <0)
RobinN7 0:3af30bfbc3e5 136 consigne_PWM1=0;
RobinN7 0:3af30bfbc3e5 137 // Moteur droit
RobinN7 0:3af30bfbc3e5 138 if (consigne_PWM2 > 1)
RobinN7 0:3af30bfbc3e5 139 consigne_PWM2=1;
RobinN7 0:3af30bfbc3e5 140 else if (consigne_PWM2 <0)
RobinN7 0:3af30bfbc3e5 141 consigne_PWM2=0;
RobinN7 0:3af30bfbc3e5 142
RobinN7 0:3af30bfbc3e5 143 // Envoi PWM aux moteurs
RobinN7 0:3af30bfbc3e5 144 Pwm_moteur_1.write(consigne_PWM1);
RobinN7 0:3af30bfbc3e5 145 Pwm_moteur_2.write(consigne_PWM2);
RobinN7 0:3af30bfbc3e5 146 }
RobinN7 0:3af30bfbc3e5 147