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@6:a4e49784b533, 2015-01-13 (annotated)

Committer:
RobinN7
Date:
Tue Jan 13 17:03:09 2015 +0000
Revision:
6:a4e49784b533
Parent:
5:4d1a524433ca
Child:
26:a836e62e0c98
ca marche pas trop mal

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 6:a4e49784b533 28 int mesure1=0,mesure_precedente1=0,mesure_moteur_1;
RobinN7 6:a4e49784b533 29 extern int consigne_moteur_1=0;
RobinN7 0:3af30bfbc3e5 30 // Coeff proportionnel, Intégral, Valeur intégrale, valeur sortie de l'asserv, consigne PWM envoyée au moteur gauche
RobinN7 0:3af30bfbc3e5 31 float Kp_moteur_1=2,Ki_moteur_1=2,I_x_moteur_1=0,sortie_asserv1,consigne_PWM1;
RobinN7 0:3af30bfbc3e5 32 // Variables d'asserv moteur droit
RobinN7 6:a4e49784b533 33 int mesure2=0,mesure_precedente2=0,mesure_moteur_2;
RobinN7 6:a4e49784b533 34 extern int consigne_moteur_2=0;
RobinN7 0:3af30bfbc3e5 35 // Coeff proportionnel, Intégral, Valeur intégrale, valeur sortie de l'asserv, consigne PWM envoyée au moteur droit
RobinN7 0:3af30bfbc3e5 36 float Kp_moteur_2=2,Ki_moteur_2=2,I_x_moteur_2=0,sortie_asserv2,consigne_PWM2;
RobinN7 0:3af30bfbc3e5 37
RobinN7 0:3af30bfbc3e5 38 // Pins de sortie PWM
RobinN7 0:3af30bfbc3e5 39
RobinN7 0:3af30bfbc3e5 40
RobinN7 0:3af30bfbc3e5 41 double i=0.4;
RobinN7 0:3af30bfbc3e5 42 /*
RobinN7 0:3af30bfbc3e5 43 void move_servo()
RobinN7 0:3af30bfbc3e5 44 {
RobinN7 0:3af30bfbc3e5 45
RobinN7 0:3af30bfbc3e5 46 float mem;
RobinN7 0:3af30bfbc3e5 47 mem=FLAG.read();
RobinN7 0:3af30bfbc3e5 48 FLAG.write(0.5);
RobinN7 0:3af30bfbc3e5 49
RobinN7 0:3af30bfbc3e5 50 if (i<0.7) i+=0.001;
RobinN7 0:3af30bfbc3e5 51 else i=0.4;
RobinN7 0:3af30bfbc3e5 52 servo = i;
RobinN7 0:3af30bfbc3e5 53
RobinN7 0:3af30bfbc3e5 54 FLAG.write(mem);
RobinN7 0:3af30bfbc3e5 55 }
RobinN7 0:3af30bfbc3e5 56 */
RobinN7 0:3af30bfbc3e5 57 void mesure_interruption(void)
RobinN7 0:3af30bfbc3e5 58 {
RobinN7 0:3af30bfbc3e5 59 float mem;
RobinN7 0:3af30bfbc3e5 60 mem=FLAG.read();
RobinN7 0:3af30bfbc3e5 61 FLAG.write(1);
RobinN7 0:3af30bfbc3e5 62 // Calcul de l'écart pour le moteur gauche
RobinN7 0:3af30bfbc3e5 63 mesure1=decodeur_1.getPulses();
RobinN7 0:3af30bfbc3e5 64 if (mesure1 >= mesure_precedente1)
RobinN7 0:3af30bfbc3e5 65 mesure_moteur_1=mesure1-mesure_precedente1;
RobinN7 0:3af30bfbc3e5 66 else
RobinN7 0:3af30bfbc3e5 67 mesure_moteur_1=mesure1-mesure_precedente1+32767;
RobinN7 0:3af30bfbc3e5 68 mesure_precedente1=mesure1;
RobinN7 0:3af30bfbc3e5 69
RobinN7 0:3af30bfbc3e5 70 // Calcul de l'écart pour le moteur droit
RobinN7 0:3af30bfbc3e5 71 mesure2=decodeur_2.getPulses();
RobinN7 0:3af30bfbc3e5 72 if (mesure2 >= mesure_precedente2)
RobinN7 0:3af30bfbc3e5 73 mesure_moteur_2=mesure2-mesure_precedente2;
RobinN7 0:3af30bfbc3e5 74 else
RobinN7 0:3af30bfbc3e5 75 mesure_moteur_2=mesure2-mesure_precedente2+32767;
RobinN7 0:3af30bfbc3e5 76 mesure_precedente2=mesure2;
RobinN7 0:3af30bfbc3e5 77
RobinN7 0:3af30bfbc3e5 78
RobinN7 0:3af30bfbc3e5 79 Bloc_propulsion();
RobinN7 0:3af30bfbc3e5 80 FLAG.write(mem);
RobinN7 0:3af30bfbc3e5 81 }
RobinN7 0:3af30bfbc3e5 82 void init_led(void)
RobinN7 0:3af30bfbc3e5 83 {
RobinN7 0:3af30bfbc3e5 84 // Ticker asserv périodique :
RobinN7 0:3af30bfbc3e5 85 //timeout.attach_us(&retard_servo,10);
RobinN7 0:3af30bfbc3e5 86 //ticker_servo.attach_us(&move_servo,0.05*1000000);
RobinN7 0:3af30bfbc3e5 87 ticker_asserv.attach_us(&mesure_interruption,Periode_echantillonage*1000000);
RobinN7 0:3af30bfbc3e5 88 led_red=1;
RobinN7 0:3af30bfbc3e5 89 led_green=1;
RobinN7 0:3af30bfbc3e5 90 Pwm_moteur_1.period_us(100);
RobinN7 0:3af30bfbc3e5 91 Pwm_moteur_2.period_us(100);
RobinN7 0:3af30bfbc3e5 92 //
RobinN7 0:3af30bfbc3e5 93 }
RobinN7 0:3af30bfbc3e5 94
RobinN7 0:3af30bfbc3e5 95
RobinN7 0:3af30bfbc3e5 96 double Regulateur_PI_decodeur(double consigne,double mesure, double Kp, double Ki ,double Periode_echantillonage,double I_x )
RobinN7 0:3af30bfbc3e5 97 {
RobinN7 0:3af30bfbc3e5 98 // Kp Ki sont des parametres Globals modifiable via la transmission série
RobinN7 0:3af30bfbc3e5 99 // Periode_echantillonage est une constante à tous le système
RobinN7 0:3af30bfbc3e5 100 // I_x est une valeur interne du regulateur
RobinN7 0:3af30bfbc3e5 101 // la consigne est la vitesse calculé par le calculateur moteur
RobinN7 0:3af30bfbc3e5 102 // la mesure est la vitesse mesuré par les qei
RobinN7 0:3af30bfbc3e5 103
RobinN7 0:3af30bfbc3e5 104 //Consigne et Mesure doivent être dans les mêm unité : rad/s
RobinN7 0:3af30bfbc3e5 105 double ecart, P_x,commande;
RobinN7 0:3af30bfbc3e5 106 /////// Régulation PI ///////
RobinN7 0:3af30bfbc3e5 107 // Ecart entre la consigne et la mesure
RobinN7 0:3af30bfbc3e5 108 ecart = consigne - mesure;
RobinN7 0:3af30bfbc3e5 109
RobinN7 0:3af30bfbc3e5 110 // Terme proportionnel
RobinN7 0:3af30bfbc3e5 111 P_x = Kp * ecart;
RobinN7 0:3af30bfbc3e5 112
RobinN7 0:3af30bfbc3e5 113 // Calcul de la commande
RobinN7 0:3af30bfbc3e5 114 commande = P_x + I_x;
RobinN7 0:3af30bfbc3e5 115
RobinN7 0:3af30bfbc3e5 116 // Terme intégral (sera utilisé lors du pas d'échantillonnage suivant)
RobinN7 0:3af30bfbc3e5 117 I_x = I_x + Ki * Periode_echantillonage * ecart;
RobinN7 0:3af30bfbc3e5 118 /////// Fin régulation PID ///////
RobinN7 0:3af30bfbc3e5 119 return commande;
RobinN7 0:3af30bfbc3e5 120 }
RobinN7 0:3af30bfbc3e5 121
RobinN7 0:3af30bfbc3e5 122 void Bloc_propulsion()
RobinN7 0:3af30bfbc3e5 123 {
RobinN7 0:3af30bfbc3e5 124 //
RobinN7 0:3af30bfbc3e5 125 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 126 //
RobinN7 0:3af30bfbc3e5 127 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 128
RobinN7 0:3af30bfbc3e5 129 //Conversion en PWM
RobinN7 0:3af30bfbc3e5 130 consigne_PWM1=sortie_asserv1/20;
RobinN7 0:3af30bfbc3e5 131 consigne_PWM2=sortie_asserv2/20;
RobinN7 0:3af30bfbc3e5 132
RobinN7 0:3af30bfbc3e5 133 // Saturation
RobinN7 0:3af30bfbc3e5 134 // Moteur gauche
RobinN7 0:3af30bfbc3e5 135 if (consigne_PWM1 > 1)
RobinN7 0:3af30bfbc3e5 136 consigne_PWM1=1;
RobinN7 0:3af30bfbc3e5 137 else if (consigne_PWM1 <0)
RobinN7 0:3af30bfbc3e5 138 consigne_PWM1=0;
RobinN7 0:3af30bfbc3e5 139 // Moteur droit
RobinN7 0:3af30bfbc3e5 140 if (consigne_PWM2 > 1)
RobinN7 0:3af30bfbc3e5 141 consigne_PWM2=1;
RobinN7 0:3af30bfbc3e5 142 else if (consigne_PWM2 <0)
RobinN7 0:3af30bfbc3e5 143 consigne_PWM2=0;
RobinN7 0:3af30bfbc3e5 144
RobinN7 0:3af30bfbc3e5 145 // Envoi PWM aux moteurs
RobinN7 0:3af30bfbc3e5 146 Pwm_moteur_1.write(consigne_PWM1);
RobinN7 0:3af30bfbc3e5 147 Pwm_moteur_2.write(consigne_PWM2);
RobinN7 0:3af30bfbc3e5 148 }
RobinN7 0:3af30bfbc3e5 149