yacine sidhoum / Mbed 2 deprecated mbed__motor_QEIHWv2_interupt_timer_sy_2017_RD_ver020

position and speed dc motor regulator with real time software and trajectory generator

Dependencies:   NVIC_set_all_priorities QEI_hw SoftPWM mbed

/media/uploads/exarkun/wp_20171120_002-1-.jpg /media/uploads/exarkun/wp_20171030_004.jpg /media/uploads/exarkun/wp_20171030_002.jpg

position/time graph: /media/uploads/exarkun/wp_20171120_004.jpg

/media/uploads/exarkun/wp_20171128_001.jpg

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?

UserRevisionLine numberNew 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 }