position and speed dc motor regulator with real time software and trajectory generator
Dependencies: NVIC_set_all_priorities QEI_hw SoftPWM mbed
position/time graph:
un grand merci a Pierre Voiriot pour son aide .
rem: pour la modiffication hardware QEI voir ce lien
https://os.mbed.com/users/hexley/notebook/qei_hw-interface-implementation-notes/
main.cpp@0:6aa00967963c, 2017-11-13 (annotated)
- Committer:
- exarkun
- Date:
- Mon Nov 13 15:44:15 2017 +0000
- Revision:
- 0:6aa00967963c
position and speed motor dc control with lmd18200t with trajectory generator best project done up to now for dc motor i used hardware QEI; you sould make some hardware modification on mbed lpc1768 ; ; ; good end year 2017 ;-)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
exarkun | 0:6aa00967963c | 1 | #include "mbed.h" |
exarkun | 0:6aa00967963c | 2 | #include "qeihw.h" |
exarkun | 0:6aa00967963c | 3 | #include "NVIC_set_all_priorities.h" |
exarkun | 0:6aa00967963c | 4 | // déclaration du hardware |
exarkun | 0:6aa00967963c | 5 | Serial pc(USBTX, USBRX);//utilisation de la liaison usb |
exarkun | 0:6aa00967963c | 6 | QEIHW qei(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_4X, QEI_INVINX_NONE ); |
exarkun | 0:6aa00967963c | 7 | Serial device(p9, p10);// tx, rx.//definition de la liaison rs232 |
exarkun | 0:6aa00967963c | 8 | DigitalOut frein_moteur1(p15); //changement momentané pour le projet r2d de p5 a p15 |
exarkun | 0:6aa00967963c | 9 | DigitalOut sens_rotation_moteur1(p13); |
exarkun | 0:6aa00967963c | 10 | PwmOut PWM1(p21); |
exarkun | 0:6aa00967963c | 11 | |
exarkun | 0:6aa00967963c | 12 | // définition des timers. |
exarkun | 0:6aa00967963c | 13 | Timer timer1; |
exarkun | 0:6aa00967963c | 14 | Timer timer2; |
exarkun | 0:6aa00967963c | 15 | |
exarkun | 0:6aa00967963c | 16 | // définition des sortie leds |
exarkun | 0:6aa00967963c | 17 | DigitalOut led1(LED1); |
exarkun | 0:6aa00967963c | 18 | DigitalOut led3(LED3); |
exarkun | 0:6aa00967963c | 19 | |
exarkun | 0:6aa00967963c | 20 | char c; |
exarkun | 0:6aa00967963c | 21 | int t ; |
exarkun | 0:6aa00967963c | 22 | |
exarkun | 0:6aa00967963c | 23 | float PI=3.141592653589793; |
exarkun | 0:6aa00967963c | 24 | |
exarkun | 0:6aa00967963c | 25 | //position codeur en pulses 2000pulses/rotation |
exarkun | 0:6aa00967963c | 26 | int position_actuelle;//position du codeur |
exarkun | 0:6aa00967963c | 27 | int position_actuelle_n1;//position du codeur a n-1 |
exarkun | 0:6aa00967963c | 28 | |
exarkun | 0:6aa00967963c | 29 | //données mecanique & moteur |
exarkun | 0:6aa00967963c | 30 | float J=3.375*1e-5;//0.005;//inertie en kg.m² |
exarkun | 0:6aa00967963c | 31 | float K=0.05;//2;//constante de couple N.m/v |
exarkun | 0:6aa00967963c | 32 | float R=5;//K;//resistance du moteur en Ohms |
exarkun | 0:6aa00967963c | 33 | float Umax=24;//tension max en volts |
exarkun | 0:6aa00967963c | 34 | |
exarkun | 0:6aa00967963c | 35 | //parametres reglage |
exarkun | 0:6aa00967963c | 36 | float dt_vit(5*1e-4); |
exarkun | 0:6aa00967963c | 37 | float Ki=10;//default value=10; |
exarkun | 0:6aa00967963c | 38 | float Kp; |
exarkun | 0:6aa00967963c | 39 | float Kv; |
exarkun | 0:6aa00967963c | 40 | float K_sw(K); |
exarkun | 0:6aa00967963c | 41 | float tau; |
exarkun | 0:6aa00967963c | 42 | float taud=(10*dt_vit); |
exarkun | 0:6aa00967963c | 43 | float r1=3;//default value=2.5; |
exarkun | 0:6aa00967963c | 44 | float r2=r1;//default value=2.5; |
exarkun | 0:6aa00967963c | 45 | |
exarkun | 0:6aa00967963c | 46 | //variables generateur de consigne/////////////////////////////////////////////////////////////////////////////////////////// |
exarkun | 0:6aa00967963c | 47 | float Teta_debut, Teta_fin, Teta_diff; |
exarkun | 0:6aa00967963c | 48 | float delta_Teta; |
exarkun | 0:6aa00967963c | 49 | float consigne;//en pulse |
exarkun | 0:6aa00967963c | 50 | float slope_time(1.);//en seconde |
exarkun | 0:6aa00967963c | 51 | bool start_slope(false);//flag debut de montee |
exarkun | 0:6aa00967963c | 52 | int ticks_total; |
exarkun | 0:6aa00967963c | 53 | int count_ticks; |
exarkun | 0:6aa00967963c | 54 | int count_slope(0); |
exarkun | 0:6aa00967963c | 55 | int count_max(0); |
exarkun | 0:6aa00967963c | 56 | |
exarkun | 0:6aa00967963c | 57 | |
exarkun | 0:6aa00967963c | 58 | //variables calcul de l'asservissement en vitesse et position |
exarkun | 0:6aa00967963c | 59 | float Tetaco; //valeur de la consigne position en radians |
exarkun | 0:6aa00967963c | 60 | float Tetam;//valeur mesuree de la position en radians |
exarkun | 0:6aa00967963c | 61 | |
exarkun | 0:6aa00967963c | 62 | float dTetam; |
exarkun | 0:6aa00967963c | 63 | float integ; |
exarkun | 0:6aa00967963c | 64 | float A; |
exarkun | 0:6aa00967963c | 65 | |
exarkun | 0:6aa00967963c | 66 | float dTetam_n1; |
exarkun | 0:6aa00967963c | 67 | float Tetaco_n1; |
exarkun | 0:6aa00967963c | 68 | float Tetam_n1; |
exarkun | 0:6aa00967963c | 69 | float integ_n1; |
exarkun | 0:6aa00967963c | 70 | float cyclic; |
exarkun | 0:6aa00967963c | 71 | int signe_rot; |
exarkun | 0:6aa00967963c | 72 | |
exarkun | 0:6aa00967963c | 73 | //declaration des differantes taches |
exarkun | 0:6aa00967963c | 74 | void task1_switch(void); |
exarkun | 0:6aa00967963c | 75 | void task2_switch(void); |
exarkun | 0:6aa00967963c | 76 | void task3_switch(void); |
exarkun | 0:6aa00967963c | 77 | void task4_switch(void); |
exarkun | 0:6aa00967963c | 78 | void task5_switch(void); |
exarkun | 0:6aa00967963c | 79 | void task6_switch(void); |
exarkun | 0:6aa00967963c | 80 | void task7_switch(void); |
exarkun | 0:6aa00967963c | 81 | void task8_switch(void); |
exarkun | 0:6aa00967963c | 82 | |
exarkun | 0:6aa00967963c | 83 | //declaration des differantes interuption timer |
exarkun | 0:6aa00967963c | 84 | Ticker time_up1; //define a Ticker, with name “time_up1” |
exarkun | 0:6aa00967963c | 85 | Ticker time_up2; //define a Ticker, with name “time_up2” |
exarkun | 0:6aa00967963c | 86 | Ticker time_up3; //define a Ticker, with name “time_up3” |
exarkun | 0:6aa00967963c | 87 | Ticker time_up4; //define a Ticker, with name “time_up4” |
exarkun | 0:6aa00967963c | 88 | Ticker time_up5; //define a Ticker, with name “time_up5” |
exarkun | 0:6aa00967963c | 89 | Ticker time_up6; //define a Ticker, with name “time_up6” |
exarkun | 0:6aa00967963c | 90 | Ticker time_up7; //define a Ticker, with name “time_up7” |
exarkun | 0:6aa00967963c | 91 | Ticker time_up8; //define a Ticker, with name “time_up8” |
exarkun | 0:6aa00967963c | 92 | |
exarkun | 0:6aa00967963c | 93 | //declaration des leds visuelle utiliser pour mesurer le temps des taches |
exarkun | 0:6aa00967963c | 94 | DigitalOut myled1(LED1); |
exarkun | 0:6aa00967963c | 95 | DigitalOut myled3(LED3); |
exarkun | 0:6aa00967963c | 96 | |
exarkun | 0:6aa00967963c | 97 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 98 | // Convertion pulses to radians // |
exarkun | 0:6aa00967963c | 99 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 100 | |
exarkun | 0:6aa00967963c | 101 | //PI =3.141592653589793 =1000 pulses codeurs |
exarkun | 0:6aa00967963c | 102 | float pulsesToRadians(int pulses) |
exarkun | 0:6aa00967963c | 103 | { |
exarkun | 0:6aa00967963c | 104 | float radians_VAL; |
exarkun | 0:6aa00967963c | 105 | radians_VAL=(pulses*PI)/1000.; |
exarkun | 0:6aa00967963c | 106 | return radians_VAL; |
exarkun | 0:6aa00967963c | 107 | }; |
exarkun | 0:6aa00967963c | 108 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 109 | // calcule de la vitesse angulaire // |
exarkun | 0:6aa00967963c | 110 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 111 | |
exarkun | 0:6aa00967963c | 112 | // ici le code du calcule |
exarkun | 0:6aa00967963c | 113 | |
exarkun | 0:6aa00967963c | 114 | // Convert ASCII string to unsigned 32-bit decimal "string is null-terminated" |
exarkun | 0:6aa00967963c | 115 | unsigned long Str2UDec(unsigned char string[]){ |
exarkun | 0:6aa00967963c | 116 | unsigned long i = 0; // index |
exarkun | 0:6aa00967963c | 117 | unsigned long n = 0; // number |
exarkun | 0:6aa00967963c | 118 | while(string[i] != 0){ |
exarkun | 0:6aa00967963c | 119 | n = 10*n +(string[i]-0x30); |
exarkun | 0:6aa00967963c | 120 | i++; |
exarkun | 0:6aa00967963c | 121 | } |
exarkun | 0:6aa00967963c | 122 | return n; |
exarkun | 0:6aa00967963c | 123 | } |
exarkun | 0:6aa00967963c | 124 | |
exarkun | 0:6aa00967963c | 125 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 126 | // TASKS1 // |
exarkun | 0:6aa00967963c | 127 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 128 | |
exarkun | 0:6aa00967963c | 129 | void task1_switch() |
exarkun | 0:6aa00967963c | 130 | { |
exarkun | 0:6aa00967963c | 131 | myled1=1; |
exarkun | 0:6aa00967963c | 132 | //lecture valeur codeur et conversion en radians |
exarkun | 0:6aa00967963c | 133 | position_actuelle=qei.GetPosition();//lecture valeur codeur et affectation a la variable globale |
exarkun | 0:6aa00967963c | 134 | Tetam=pulsesToRadians(position_actuelle);//conversion valeur impulsionel en radians |
exarkun | 0:6aa00967963c | 135 | |
exarkun | 0:6aa00967963c | 136 | //Calcul de la nouvelle consigne: |
exarkun | 0:6aa00967963c | 137 | |
exarkun | 0:6aa00967963c | 138 | //Etape1:derivee filtree |
exarkun | 0:6aa00967963c | 139 | dTetam=1./(1.+taud*2./dt_vit)*(-dTetam_n1*(1.-taud*2./dt_vit)+2./dt_vit*(Tetam-Tetam_n1)); |
exarkun | 0:6aa00967963c | 140 | |
exarkun | 0:6aa00967963c | 141 | //Etape2:calcul de integ non saturee |
exarkun | 0:6aa00967963c | 142 | integ=integ_n1+dt_vit/2.*((Tetaco-Tetam)+(Tetaco_n1-Tetam_n1)); |
exarkun | 0:6aa00967963c | 143 | |
exarkun | 0:6aa00967963c | 144 | //Etape3:Calcul de A non saturee |
exarkun | 0:6aa00967963c | 145 | A=Kv*K_sw/Umax*(-dTetam+Kp*Ki*integ-Kp*Tetam); |
exarkun | 0:6aa00967963c | 146 | |
exarkun | 0:6aa00967963c | 147 | //Etape 4 et 5 : calcul de integ saturee |
exarkun | 0:6aa00967963c | 148 | if (A>1) |
exarkun | 0:6aa00967963c | 149 | { |
exarkun | 0:6aa00967963c | 150 | integ=1./Kp/Ki*(Umax/Kv/K_sw+dTetam+Kp*Tetam); |
exarkun | 0:6aa00967963c | 151 | A=1; |
exarkun | 0:6aa00967963c | 152 | } |
exarkun | 0:6aa00967963c | 153 | if(A<-1) |
exarkun | 0:6aa00967963c | 154 | { |
exarkun | 0:6aa00967963c | 155 | integ=1./Kp/Ki*(-Umax/Kv/K_sw+dTetam+Kp*Tetam); |
exarkun | 0:6aa00967963c | 156 | A=-1; |
exarkun | 0:6aa00967963c | 157 | } |
exarkun | 0:6aa00967963c | 158 | |
exarkun | 0:6aa00967963c | 159 | //Etape 6:affectation du signe de rotation a l'etage de puissance |
exarkun | 0:6aa00967963c | 160 | if (A>0) |
exarkun | 0:6aa00967963c | 161 | { |
exarkun | 0:6aa00967963c | 162 | signe_rot=1; |
exarkun | 0:6aa00967963c | 163 | sens_rotation_moteur1.write(signe_rot); //affectation du sens de rotation moteur1 |
exarkun | 0:6aa00967963c | 164 | cyclic=A; |
exarkun | 0:6aa00967963c | 165 | } |
exarkun | 0:6aa00967963c | 166 | else |
exarkun | 0:6aa00967963c | 167 | { |
exarkun | 0:6aa00967963c | 168 | signe_rot=0; |
exarkun | 0:6aa00967963c | 169 | sens_rotation_moteur1.write(signe_rot); //affectation du sens de rotation moteur1 |
exarkun | 0:6aa00967963c | 170 | cyclic=-A;//peut etre une erreur ici sy?? |
exarkun | 0:6aa00967963c | 171 | } |
exarkun | 0:6aa00967963c | 172 | |
exarkun | 0:6aa00967963c | 173 | PWM1.write(cyclic);// affectation de la valeur calculé en pwm |
exarkun | 0:6aa00967963c | 174 | |
exarkun | 0:6aa00967963c | 175 | // enregistrement des valeurs N-1 |
exarkun | 0:6aa00967963c | 176 | position_actuelle_n1=position_actuelle; |
exarkun | 0:6aa00967963c | 177 | dTetam_n1=dTetam; |
exarkun | 0:6aa00967963c | 178 | Tetaco_n1=Tetaco; |
exarkun | 0:6aa00967963c | 179 | Tetam_n1=Tetam; |
exarkun | 0:6aa00967963c | 180 | integ_n1=integ; |
exarkun | 0:6aa00967963c | 181 | |
exarkun | 0:6aa00967963c | 182 | myled1=0; |
exarkun | 0:6aa00967963c | 183 | //myled1=!myled1;//changement etat de la led1 |
exarkun | 0:6aa00967963c | 184 | } |
exarkun | 0:6aa00967963c | 185 | |
exarkun | 0:6aa00967963c | 186 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 187 | // TASKS2 // |
exarkun | 0:6aa00967963c | 188 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 189 | |
exarkun | 0:6aa00967963c | 190 | void task2_switch() |
exarkun | 0:6aa00967963c | 191 | { |
exarkun | 0:6aa00967963c | 192 | // pc.printf("%f\r\n", Tetaco);//valeur float consigne en radians le renvoyé par usb |
exarkun | 0:6aa00967963c | 193 | // pc.printf("%i\r\n", position_actuelle);//valeur int du codeur renvoyé par usb |
exarkun | 0:6aa00967963c | 194 | // pc.printf("%f\r\n", Tetam);//valeur float du codeur en radians renvoyé par usb |
exarkun | 0:6aa00967963c | 195 | // pc.printf("dTetam : %f\r\n", dTetam);//valeur float dTetam |
exarkun | 0:6aa00967963c | 196 | // pc.printf("integ : %f\r\n", integ);//valeur float integ |
exarkun | 0:6aa00967963c | 197 | // pc.printf("%f\r\n", A);//valeur float du codeur en radians le renvoyé par usb |
exarkun | 0:6aa00967963c | 198 | // pc.printf("%e\r\n", cyclic);//valeur float du codeur en radians le renvoyé par usb |
exarkun | 0:6aa00967963c | 199 | // pc.printf("\r\n");//retour chario |
exarkun | 0:6aa00967963c | 200 | // pc.printf("$%d %d %d;", position_actuelle,position_actuelle/2,position_actuelle/10 ); //utiliser avec logicielle serial port ploter |
exarkun | 0:6aa00967963c | 201 | pc.printf("$%d;", position_actuelle); //utiliser avec logicielle serial port ploter |
exarkun | 0:6aa00967963c | 202 | |
exarkun | 0:6aa00967963c | 203 | }; |
exarkun | 0:6aa00967963c | 204 | |
exarkun | 0:6aa00967963c | 205 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 206 | // TASKS3 // |
exarkun | 0:6aa00967963c | 207 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 208 | |
exarkun | 0:6aa00967963c | 209 | void task3_switch() |
exarkun | 0:6aa00967963c | 210 | { |
exarkun | 0:6aa00967963c | 211 | myled3=1; |
exarkun | 0:6aa00967963c | 212 | |
exarkun | 0:6aa00967963c | 213 | //generation de la tragectoire |
exarkun | 0:6aa00967963c | 214 | if(start_slope) |
exarkun | 0:6aa00967963c | 215 | { |
exarkun | 0:6aa00967963c | 216 | Teta_debut=Tetam;//affectation de la consigne a la variable globale |
exarkun | 0:6aa00967963c | 217 | Teta_fin=pulsesToRadians(consigne);//affectation de la consigne a la variable globale |
exarkun | 0:6aa00967963c | 218 | Teta_diff = Teta_fin - Teta_debut; |
exarkun | 0:6aa00967963c | 219 | delta_Teta=(Teta_diff/slope_time)*5*1e-4; |
exarkun | 0:6aa00967963c | 220 | //timer1.start();// déclenchement du timer1 |
exarkun | 0:6aa00967963c | 221 | count_slope = 0; |
exarkun | 0:6aa00967963c | 222 | count_max = slope_time/(5*1e-4); |
exarkun | 0:6aa00967963c | 223 | start_slope=false; |
exarkun | 0:6aa00967963c | 224 | } |
exarkun | 0:6aa00967963c | 225 | //float timeco = timer1.read(); |
exarkun | 0:6aa00967963c | 226 | count_slope++; |
exarkun | 0:6aa00967963c | 227 | if (count_slope<=count_max){Tetaco=Tetaco+delta_Teta;} |
exarkun | 0:6aa00967963c | 228 | //if (timeco<slope_time){Tetaco=Tetaco+delta_Teta;} |
exarkun | 0:6aa00967963c | 229 | //if (timeco==slope_time){Tetaco=Teta_fin;} |
exarkun | 0:6aa00967963c | 230 | //if (timeco>slope_time){timer1.reset(); // remise à zéro du timer1 |
exarkun | 0:6aa00967963c | 231 | |
exarkun | 0:6aa00967963c | 232 | //myled3=!myled3; |
exarkun | 0:6aa00967963c | 233 | myled3=0; |
exarkun | 0:6aa00967963c | 234 | } |
exarkun | 0:6aa00967963c | 235 | |
exarkun | 0:6aa00967963c | 236 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 237 | // TASKS4 // |
exarkun | 0:6aa00967963c | 238 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 239 | |
exarkun | 0:6aa00967963c | 240 | void task4_switch() |
exarkun | 0:6aa00967963c | 241 | { |
exarkun | 0:6aa00967963c | 242 | device.printf("$%d;", position_actuelle); //utiliser avec logicielle serial port ploter |
exarkun | 0:6aa00967963c | 243 | |
exarkun | 0:6aa00967963c | 244 | } |
exarkun | 0:6aa00967963c | 245 | |
exarkun | 0:6aa00967963c | 246 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 247 | // TASKS5 // |
exarkun | 0:6aa00967963c | 248 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 249 | |
exarkun | 0:6aa00967963c | 250 | void task5_switch() |
exarkun | 0:6aa00967963c | 251 | { |
exarkun | 0:6aa00967963c | 252 | // ici code de la tache 5 |
exarkun | 0:6aa00967963c | 253 | } |
exarkun | 0:6aa00967963c | 254 | |
exarkun | 0:6aa00967963c | 255 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 256 | // TASKS6 // |
exarkun | 0:6aa00967963c | 257 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 258 | |
exarkun | 0:6aa00967963c | 259 | void task6_switch() |
exarkun | 0:6aa00967963c | 260 | { |
exarkun | 0:6aa00967963c | 261 | // ici code de la tache 6 |
exarkun | 0:6aa00967963c | 262 | } |
exarkun | 0:6aa00967963c | 263 | |
exarkun | 0:6aa00967963c | 264 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 265 | // TASKS7 // |
exarkun | 0:6aa00967963c | 266 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 267 | |
exarkun | 0:6aa00967963c | 268 | void task7_switch() |
exarkun | 0:6aa00967963c | 269 | { |
exarkun | 0:6aa00967963c | 270 | // ici code de la tache 7 |
exarkun | 0:6aa00967963c | 271 | } |
exarkun | 0:6aa00967963c | 272 | |
exarkun | 0:6aa00967963c | 273 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 274 | // TASKS8 // |
exarkun | 0:6aa00967963c | 275 | //////////////////////////////////////// |
exarkun | 0:6aa00967963c | 276 | |
exarkun | 0:6aa00967963c | 277 | void task8_switch() |
exarkun | 0:6aa00967963c | 278 | { |
exarkun | 0:6aa00967963c | 279 | // ici code de la tache 8 |
exarkun | 0:6aa00967963c | 280 | } |
exarkun | 0:6aa00967963c | 281 | |
exarkun | 0:6aa00967963c | 282 | ///////////////////////////////////////////////////////////////////////////// |
exarkun | 0:6aa00967963c | 283 | // declaration de la fonction moteur1 // |
exarkun | 0:6aa00967963c | 284 | ///////////////////////////////////////////////////////////////////////////// |
exarkun | 0:6aa00967963c | 285 | |
exarkun | 0:6aa00967963c | 286 | void motor1_mouve(int position_final,int T1) |
exarkun | 0:6aa00967963c | 287 | { |
exarkun | 0:6aa00967963c | 288 | //calcule nombres de ticks tache2 suite au temps T1 demander |
exarkun | 0:6aa00967963c | 289 | |
exarkun | 0:6aa00967963c | 290 | //calcule delta_Teta |
exarkun | 0:6aa00967963c | 291 | |
exarkun | 0:6aa00967963c | 292 | |
exarkun | 0:6aa00967963c | 293 | /////////////////////////////////////////////////// |
exarkun | 0:6aa00967963c | 294 | // debut du mouvement // |
exarkun | 0:6aa00967963c | 295 | /////////////////////////////////////////////////// |
exarkun | 0:6aa00967963c | 296 | |
exarkun | 0:6aa00967963c | 297 | timer1.start(); // déclenchement du timer1 |
exarkun | 0:6aa00967963c | 298 | timer2.start(); // déclenchement du timer2 |
exarkun | 0:6aa00967963c | 299 | |
exarkun | 0:6aa00967963c | 300 | //generation de consigne position |
exarkun | 0:6aa00967963c | 301 | for(int i=0;i<=consigne;i++) |
exarkun | 0:6aa00967963c | 302 | { |
exarkun | 0:6aa00967963c | 303 | } |
exarkun | 0:6aa00967963c | 304 | if (timer1.read_us()>T1) // lecture du temps du timer1 en us |
exarkun | 0:6aa00967963c | 305 | { |
exarkun | 0:6aa00967963c | 306 | device.printf("TIME out motor1 0\r\n"); |
exarkun | 0:6aa00967963c | 307 | } |
exarkun | 0:6aa00967963c | 308 | |
exarkun | 0:6aa00967963c | 309 | if (timer2.read_ms()>T1) // lecture du temps du timer1 en ms |
exarkun | 0:6aa00967963c | 310 | { |
exarkun | 0:6aa00967963c | 311 | device.printf("TIME out motor1 0\r\n"); |
exarkun | 0:6aa00967963c | 312 | } |
exarkun | 0:6aa00967963c | 313 | timer1.reset(); // remise à zéro du timer1 |
exarkun | 0:6aa00967963c | 314 | timer2.reset(); // remise à zéro du timer2 |
exarkun | 0:6aa00967963c | 315 | }; |
exarkun | 0:6aa00967963c | 316 | |
exarkun | 0:6aa00967963c | 317 | //////////////////////////////////////////////////////////////////////////// |
exarkun | 0:6aa00967963c | 318 | // PROGRAMME PRINCIPAL // |
exarkun | 0:6aa00967963c | 319 | //////////////////////////////////////////////////////////////////////////// |
exarkun | 0:6aa00967963c | 320 | |
exarkun | 0:6aa00967963c | 321 | int main() |
exarkun | 0:6aa00967963c | 322 | { |
exarkun | 0:6aa00967963c | 323 | int cycles; |
exarkun | 0:6aa00967963c | 324 | //NVIC_SetPriority(EINT3_IRQn, 252);//set interupt to highest priority 0. |
exarkun | 0:6aa00967963c | 325 | //NVIC_SetPriority(TIMER1_IRQn, 253);// set mbed tickers to lower priority than other things |
exarkun | 0:6aa00967963c | 326 | //NVIC_SetPriority(TIMER2_IRQn, 254);// set mbed tickers to lower priority than other things |
exarkun | 0:6aa00967963c | 327 | //NVIC_SetPriority(TIMER3_IRQn, 255);// set mbed tickers to lower priority than other things |
exarkun | 0:6aa00967963c | 328 | //NVIC_set_all_irq_priorities(0); |
exarkun | 0:6aa00967963c | 329 | |
exarkun | 0:6aa00967963c | 330 | qei.SetDigiFilter(480UL);//filtre |
exarkun | 0:6aa00967963c | 331 | qei.SetMaxPosition(0xFFFFFFFF);//position max 4294967295 pulses |
exarkun | 0:6aa00967963c | 332 | |
exarkun | 0:6aa00967963c | 333 | //initialisation de la com rs232 |
exarkun | 0:6aa00967963c | 334 | device.baud(9600);//rs232 28800 baud |
exarkun | 0:6aa00967963c | 335 | device.printf("serial rs232 ok \n"); |
exarkun | 0:6aa00967963c | 336 | |
exarkun | 0:6aa00967963c | 337 | //calculs de tau ,Kv,Kp |
exarkun | 0:6aa00967963c | 338 | tau=(J*R)/K; |
exarkun | 0:6aa00967963c | 339 | Kv=r1*r2*tau*Ki-1.; |
exarkun | 0:6aa00967963c | 340 | if(Kv<0){Kv = -Kv;} |
exarkun | 0:6aa00967963c | 341 | Kp=r1*Ki*(1.+Kv)/Kv; |
exarkun | 0:6aa00967963c | 342 | |
exarkun | 0:6aa00967963c | 343 | //initialisation moteur |
exarkun | 0:6aa00967963c | 344 | PWM1.period(0.00005);// initialisation du rapport cyclique fixe la période à 50us----f=20Khz |
exarkun | 0:6aa00967963c | 345 | frein_moteur1.write(0);//affectation de la valeur du frein moteur1 |
exarkun | 0:6aa00967963c | 346 | sens_rotation_moteur1.write(0); //affectation du sens de rotation moteur1 non utiliser puisque c'est la tache1 qui le fait non pas la generation de trajectoire |
exarkun | 0:6aa00967963c | 347 | |
exarkun | 0:6aa00967963c | 348 | //lancement des tasks |
exarkun | 0:6aa00967963c | 349 | time_up1.attach(&task1_switch, 0.0005); //initialises the ticker 2Khz "tous les 500us il y as une interruption task1" |
exarkun | 0:6aa00967963c | 350 | time_up2.attach(&task2_switch, 0.01); //initialises the ticker 100hz |
exarkun | 0:6aa00967963c | 351 | time_up3.attach(&task3_switch, 0.0005); //initialises the ticker 2khz |
exarkun | 0:6aa00967963c | 352 | time_up4.attach(&task4_switch, 0.01); //initialises the ticker 100hz |
exarkun | 0:6aa00967963c | 353 | time_up5.attach(&task5_switch, 0.0005); //initialises the ticker 2kh |
exarkun | 0:6aa00967963c | 354 | time_up6.attach(&task6_switch, 0.0005); //initialises the ticker 2kh |
exarkun | 0:6aa00967963c | 355 | time_up7.attach(&task7_switch, 0.0005); //initialises the ticker 2kh |
exarkun | 0:6aa00967963c | 356 | time_up8.attach(&task8_switch, 0.0005); //initialises the ticker 2kh |
exarkun | 0:6aa00967963c | 357 | |
exarkun | 0:6aa00967963c | 358 | count_ticks=0; |
exarkun | 0:6aa00967963c | 359 | while(1) |
exarkun | 0:6aa00967963c | 360 | { |
exarkun | 0:6aa00967963c | 361 | c='1'; |
exarkun | 0:6aa00967963c | 362 | //device.scanf("%c",&c);//capture du caract ascii |
exarkun | 0:6aa00967963c | 363 | |
exarkun | 0:6aa00967963c | 364 | switch(c) |
exarkun | 0:6aa00967963c | 365 | { |
exarkun | 0:6aa00967963c | 366 | case '1': |
exarkun | 0:6aa00967963c | 367 | consigne=70000; |
exarkun | 0:6aa00967963c | 368 | slope_time=0.001; |
exarkun | 0:6aa00967963c | 369 | start_slope=true; |
exarkun | 0:6aa00967963c | 370 | wait(3); |
exarkun | 0:6aa00967963c | 371 | consigne=0; |
exarkun | 0:6aa00967963c | 372 | slope_time=2.; |
exarkun | 0:6aa00967963c | 373 | start_slope=true; |
exarkun | 0:6aa00967963c | 374 | wait(3); |
exarkun | 0:6aa00967963c | 375 | |
exarkun | 0:6aa00967963c | 376 | break; |
exarkun | 0:6aa00967963c | 377 | |
exarkun | 0:6aa00967963c | 378 | case '2': |
exarkun | 0:6aa00967963c | 379 | device.printf("profile de mouvement 2\r\n"); |
exarkun | 0:6aa00967963c | 380 | break; |
exarkun | 0:6aa00967963c | 381 | |
exarkun | 0:6aa00967963c | 382 | case '3': |
exarkun | 0:6aa00967963c | 383 | device.printf("moving3 Robotic Axis 1\r\n"); |
exarkun | 0:6aa00967963c | 384 | break; |
exarkun | 0:6aa00967963c | 385 | |
exarkun | 0:6aa00967963c | 386 | case '4': |
exarkun | 0:6aa00967963c | 387 | device.printf("moving4 Robotic Axis 1\r\n"); |
exarkun | 0:6aa00967963c | 388 | break; |
exarkun | 0:6aa00967963c | 389 | |
exarkun | 0:6aa00967963c | 390 | case '5': |
exarkun | 0:6aa00967963c | 391 | device.printf("moving5 Robotic Axis 1\r\n"); |
exarkun | 0:6aa00967963c | 392 | break; |
exarkun | 0:6aa00967963c | 393 | |
exarkun | 0:6aa00967963c | 394 | case '6': |
exarkun | 0:6aa00967963c | 395 | device.printf("moving6 Robotic Axis 1\r\n"); |
exarkun | 0:6aa00967963c | 396 | break; |
exarkun | 0:6aa00967963c | 397 | |
exarkun | 0:6aa00967963c | 398 | case '7': |
exarkun | 0:6aa00967963c | 399 | device.printf("moving7 Robotic Axis 1\r\n"); |
exarkun | 0:6aa00967963c | 400 | break; |
exarkun | 0:6aa00967963c | 401 | |
exarkun | 0:6aa00967963c | 402 | case '8': |
exarkun | 0:6aa00967963c | 403 | device.printf("moving8 Robotic Axis 1\r\n"); |
exarkun | 0:6aa00967963c | 404 | break; |
exarkun | 0:6aa00967963c | 405 | |
exarkun | 0:6aa00967963c | 406 | case '9': |
exarkun | 0:6aa00967963c | 407 | device.printf("moving9 Robotic Axis 1\r\n"); |
exarkun | 0:6aa00967963c | 408 | break; |
exarkun | 0:6aa00967963c | 409 | |
exarkun | 0:6aa00967963c | 410 | case '0': |
exarkun | 0:6aa00967963c | 411 | device.printf("moving7 Robotic Axis 1\r\n"); |
exarkun | 0:6aa00967963c | 412 | break; |
exarkun | 0:6aa00967963c | 413 | } |
exarkun | 0:6aa00967963c | 414 | } |
exarkun | 0:6aa00967963c | 415 | |
exarkun | 0:6aa00967963c | 416 | } |