main.cpp

00001 #include "mbed.h"
00002 #include "qeihw.h"
00003 #include "NVIC_set_all_priorities.h"
00004 // déclaration du hardware
00005 Serial pc(USBTX, USBRX);//utilisation de la liaison usb 
00006 QEIHW qei(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_4X, QEI_INVINX_NONE );
00007 Serial device(p9, p10);// tx, rx.//definition de la liaison rs232
00008 DigitalOut frein_moteur1(p15); //changement momentané pour le projet r2d de p5 a p15
00009 DigitalOut sens_rotation_moteur1(p13);  
00010 PwmOut PWM1(p21);
00011  
00012 // définition des timers.
00013 Timer timer1; 
00014 Timer timer2;
00015 
00016 // définition des sortie leds
00017 DigitalOut led1(LED1);
00018 DigitalOut led3(LED3);
00019 
00020 char c;
00021 int t ;
00022 
00023 float PI=3.141592653589793;
00024 
00025 //position codeur en pulses 2000pulses/rotation 
00026 int position_actuelle;//position du codeur
00027 int position_actuelle_n1;//position du codeur a n-1
00028 
00029 //données mecanique & moteur 
00030 float J=3.375*1e-5;//0.005;//inertie en kg.m²
00031 float K=0.05;//2;//constante de couple  N.m/v
00032 float R=5;//K;//resistance du moteur en Ohms
00033 float Umax=24;//tension max en volts
00034 
00035 //parametres reglage
00036 float dt_vit(5*1e-4);
00037 float Ki=10;//default value=10;
00038 float Kp;
00039 float Kv;
00040 float K_sw(K);
00041 float tau;
00042 float taud=(10*dt_vit);
00043 float r1=3;//default value=2.5;
00044 float r2=r1;//default value=2.5;
00045 
00046 //variables generateur de consigne///////////////////////////////////////////////////////////////////////////////////////////
00047 float Teta_debut, Teta_fin, Teta_diff;
00048 float delta_Teta;
00049 float consigne;//en pulse 
00050 float slope_time(1.);//en seconde
00051 bool start_slope(false);//flag debut de montee
00052 int ticks_total;
00053 int count_ticks;
00054 int count_slope(0);
00055 int count_max(0);
00056 
00057 
00058 //variables calcul de l'asservissement en vitesse et position 
00059 float Tetaco; //valeur de la consigne position en radians
00060 float Tetam;//valeur mesuree de la position en radians
00061 
00062 float dTetam;
00063 float integ;
00064 float A;
00065 
00066 float dTetam_n1;
00067 float Tetaco_n1;
00068 float Tetam_n1;
00069 float integ_n1;
00070 float cyclic;
00071 int signe_rot;
00072 
00073 //declaration des differantes taches
00074 void task1_switch(void);
00075 void task2_switch(void);
00076 void task3_switch(void);
00077 void task4_switch(void);
00078 void task5_switch(void);
00079 void task6_switch(void);
00080 void task7_switch(void);
00081 void task8_switch(void);
00082 
00083 //declaration des differantes interuption timer
00084 Ticker time_up1; //define a Ticker, with name “time_up1”
00085 Ticker time_up2; //define a Ticker, with name “time_up2”
00086 Ticker time_up3; //define a Ticker, with name “time_up3”
00087 Ticker time_up4; //define a Ticker, with name “time_up4”
00088 Ticker time_up5; //define a Ticker, with name “time_up5”
00089 Ticker time_up6; //define a Ticker, with name “time_up6”
00090 Ticker time_up7; //define a Ticker, with name “time_up7”
00091 Ticker time_up8; //define a Ticker, with name “time_up8”
00092 
00093 //declaration des leds visuelle utiliser pour mesurer le temps des taches 
00094 DigitalOut myled1(LED1);
00095 DigitalOut myled3(LED3);
00096 
00097 ////////////////////////////////////////
00098 //    Convertion pulses to radians    //
00099 ////////////////////////////////////////
00100 
00101 //PI =3.141592653589793 =1000 pulses codeurs
00102 float pulsesToRadians(int pulses) 
00103 {
00104   float radians_VAL;
00105   radians_VAL=(pulses*PI)/1000.;
00106   return radians_VAL;
00107 };
00108 ////////////////////////////////////////
00109 //   calcule de la vitesse angulaire  //
00110 ////////////////////////////////////////
00111 
00112 // ici le code du calcule 
00113 
00114 // Convert ASCII string to unsigned 32-bit decimal "string is null-terminated"
00115   unsigned long Str2UDec(unsigned char string[]){
00116   unsigned long i = 0;  // index
00117   unsigned long n = 0;  // number
00118   while(string[i] != 0){
00119     n = 10*n +(string[i]-0x30);
00120     i++;
00121   }
00122   return n;
00123 }
00124 
00125 ////////////////////////////////////////
00126 //                TASKS1              //
00127 ////////////////////////////////////////
00128 
00129 void task1_switch()
00130 { 
00131 myled1=1;
00132 //lecture valeur codeur et conversion en radians
00133 position_actuelle=qei.GetPosition();//lecture valeur codeur et affectation a la variable globale 
00134 Tetam=pulsesToRadians(position_actuelle);//conversion valeur impulsionel en radians 
00135 
00136 //Calcul de la nouvelle consigne:
00137 
00138 //Etape1:derivee filtree 
00139 dTetam=1./(1.+taud*2./dt_vit)*(-dTetam_n1*(1.-taud*2./dt_vit)+2./dt_vit*(Tetam-Tetam_n1));
00140 
00141 //Etape2:calcul de integ non saturee
00142 integ=integ_n1+dt_vit/2.*((Tetaco-Tetam)+(Tetaco_n1-Tetam_n1));
00143 
00144 //Etape3:Calcul de A non saturee
00145 A=Kv*K_sw/Umax*(-dTetam+Kp*Ki*integ-Kp*Tetam);
00146 
00147 //Etape 4 et 5 : calcul de integ saturee
00148 if (A>1)
00149     {
00150     integ=1./Kp/Ki*(Umax/Kv/K_sw+dTetam+Kp*Tetam);
00151     A=1;
00152     }
00153 if(A<-1)
00154     {
00155     integ=1./Kp/Ki*(-Umax/Kv/K_sw+dTetam+Kp*Tetam); 
00156     A=-1; 
00157     }
00158     
00159 //Etape 6:affectation du signe de rotation a l'etage de puissance 
00160 if (A>0)
00161 {
00162     signe_rot=1;
00163     sens_rotation_moteur1.write(signe_rot); //affectation du sens de rotation moteur1
00164     cyclic=A;
00165 }
00166 else 
00167 {
00168     signe_rot=0;
00169     sens_rotation_moteur1.write(signe_rot); //affectation du sens de rotation moteur1
00170     cyclic=-A;//peut etre une erreur ici sy??
00171 }
00172 
00173 PWM1.write(cyclic);// affectation  de la valeur calculé en pwm
00174 
00175 //   enregistrement des valeurs N-1
00176 position_actuelle_n1=position_actuelle;   
00177 dTetam_n1=dTetam;
00178 Tetaco_n1=Tetaco;
00179 Tetam_n1=Tetam;
00180 integ_n1=integ;
00181 
00182 myled1=0;
00183 //myled1=!myled1;//changement etat de la led1
00184 }
00185 
00186 ////////////////////////////////////////
00187 //                TASKS2              //
00188 ////////////////////////////////////////
00189 
00190 void task2_switch()
00191 {
00192 //        pc.printf("%f\r\n", Tetaco);//valeur float consigne  en radians le renvoyé par usb
00193 //        pc.printf("%i\r\n", position_actuelle);//valeur int du codeur renvoyé par usb        
00194 //        pc.printf("%f\r\n", Tetam);//valeur float du codeur en radians renvoyé par usb
00195 //        pc.printf("dTetam : %f\r\n", dTetam);//valeur float dTetam
00196 //        pc.printf("integ : %f\r\n", integ);//valeur float integ               
00197 //        pc.printf("%f\r\n", A);//valeur float du codeur en radians le renvoyé par usb
00198 //        pc.printf("%e\r\n", cyclic);//valeur float du codeur en radians le renvoyé par usb
00199 //        pc.printf("\r\n");//retour chario
00200 //        pc.printf("$%d %d %d;", position_actuelle,position_actuelle/2,position_actuelle/10 ); //utiliser avec logicielle serial port ploter 
00201 pc.printf("$%d;", position_actuelle); //utiliser avec logicielle serial port ploter 
00202                  
00203 };
00204 
00205 ////////////////////////////////////////
00206 //                TASKS3              //
00207 ////////////////////////////////////////
00208 
00209 void task3_switch()
00210 { 
00211 myled3=1;
00212 
00213 //generation de la tragectoire 
00214 if(start_slope)
00215 {
00216     Teta_debut=Tetam;//affectation de la consigne a la variable globale
00217     Teta_fin=pulsesToRadians(consigne);//affectation de la consigne a la variable globale
00218     Teta_diff = Teta_fin - Teta_debut;
00219     delta_Teta=(Teta_diff/slope_time)*5*1e-4;
00220     //timer1.start();// déclenchement du timer1
00221     count_slope = 0;
00222     count_max = slope_time/(5*1e-4);
00223     start_slope=false;
00224 }
00225 //float timeco = timer1.read();
00226 count_slope++;
00227 if (count_slope<=count_max){Tetaco=Tetaco+delta_Teta;}
00228 //if (timeco<slope_time){Tetaco=Tetaco+delta_Teta;}
00229 //if (timeco==slope_time){Tetaco=Teta_fin;}
00230 //if (timeco>slope_time){timer1.reset(); // remise à zéro du timer1
00231 
00232 //myled3=!myled3;
00233 myled3=0; 
00234 }
00235 
00236 ////////////////////////////////////////
00237 //                TASKS4              //
00238 ////////////////////////////////////////
00239 
00240 void task4_switch()
00241 { 
00242 device.printf("$%d;", position_actuelle); //utiliser avec logicielle serial port ploter 
00243 
00244 }
00245 
00246 ////////////////////////////////////////
00247 //                TASKS5              //
00248 ////////////////////////////////////////
00249 
00250 void task5_switch()
00251 {
00252 // ici code de la tache 5
00253 }
00254 
00255 ////////////////////////////////////////
00256 //                TASKS6              //
00257 ////////////////////////////////////////
00258 
00259 void task6_switch()
00260 { 
00261 // ici code de la tache 6
00262 }
00263 
00264 ////////////////////////////////////////
00265 //                TASKS7              //
00266 ////////////////////////////////////////
00267 
00268 void task7_switch()
00269 { 
00270 // ici code de la tache 7
00271 }
00272 
00273 ////////////////////////////////////////
00274 //                TASKS8              //
00275 ////////////////////////////////////////
00276 
00277 void task8_switch()
00278 { 
00279 // ici code de la tache 8
00280 }
00281 
00282 /////////////////////////////////////////////////////////////////////////////
00283 //                   declaration de la fonction moteur1                    //
00284 /////////////////////////////////////////////////////////////////////////////
00285 
00286 void motor1_mouve(int position_final,int T1)
00287 {
00288 //calcule nombres de ticks tache2 suite au temps T1 demander
00289  
00290 //calcule delta_Teta
00291 
00292 
00293 ///////////////////////////////////////////////////
00294 //                debut du mouvement             //
00295 ///////////////////////////////////////////////////
00296 
00297 timer1.start(); // déclenchement du timer1 
00298 timer2.start(); // déclenchement du timer2
00299     
00300 //generation de consigne position   
00301 for(int i=0;i<=consigne;i++)
00302 {
00303 }
00304     if (timer1.read_us()>T1) // lecture du temps du timer1 en us
00305     { 
00306       device.printf("TIME out  motor1  0\r\n"); 
00307     } 
00308 
00309     if (timer2.read_ms()>T1) // lecture du temps du timer1 en ms
00310     { 
00311       device.printf("TIME out  motor1  0\r\n"); 
00312     }      
00313 timer1.reset(); // remise à zéro du timer1
00314 timer2.reset(); // remise à zéro du timer2
00315 };
00316 
00317 ////////////////////////////////////////////////////////////////////////////
00318 //                       PROGRAMME PRINCIPAL                              //
00319 ////////////////////////////////////////////////////////////////////////////
00320                                                 
00321 int main()
00322             {
00323                int cycles;
00324                 //NVIC_SetPriority(EINT3_IRQn, 252);//set interupt to highest priority 0.
00325                 //NVIC_SetPriority(TIMER1_IRQn, 253);// set mbed tickers to lower priority than other things
00326                 //NVIC_SetPriority(TIMER2_IRQn, 254);// set mbed tickers to lower priority than other things
00327                 //NVIC_SetPriority(TIMER3_IRQn, 255);// set mbed tickers to lower priority than other things
00328                 //NVIC_set_all_irq_priorities(0);
00329                 
00330                 qei.SetDigiFilter(480UL);//filtre 
00331                 qei.SetMaxPosition(0xFFFFFFFF);//position max 4294967295 pulses
00332                 
00333 //initialisation de la com rs232                
00334                 device.baud(9600);//rs232 28800 baud    
00335                 device.printf("serial rs232 ok \n");
00336                 
00337                 //calculs  de tau ,Kv,Kp          
00338                 tau=(J*R)/K;
00339                 Kv=r1*r2*tau*Ki-1.;
00340                 if(Kv<0){Kv = -Kv;}
00341                 Kp=r1*Ki*(1.+Kv)/Kv;
00342                               
00343 //initialisation moteur   
00344 PWM1.period(0.00005);// initialisation du rapport cyclique fixe la période à 50us----f=20Khz
00345 frein_moteur1.write(0);//affectation de la valeur du frein moteur1
00346 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
00347 
00348 //lancement des tasks               
00349                 time_up1.attach(&task1_switch, 0.0005); //initialises the ticker  2Khz "tous les 500us il y as une interruption task1"
00350                 time_up2.attach(&task2_switch, 0.01); //initialises the ticker 100hz
00351                 time_up3.attach(&task3_switch, 0.0005); //initialises the ticker 2khz
00352                 time_up4.attach(&task4_switch, 0.01); //initialises the ticker 100hz
00353                 time_up5.attach(&task5_switch, 0.0005); //initialises the ticker 2kh
00354                 time_up6.attach(&task6_switch, 0.0005); //initialises the ticker 2kh
00355                 time_up7.attach(&task7_switch, 0.0005); //initialises the ticker 2kh
00356                 time_up8.attach(&task8_switch, 0.0005); //initialises the ticker 2kh
00357  
00358 count_ticks=0;                      
00359         while(1)
00360                 {
00361                  c='1';  
00362                //device.scanf("%c",&c);//capture du caract ascii 
00363                      
00364                 switch(c)
00365                 {
00366                     case '1':
00367                         consigne=70000;                                  
00368                         slope_time=0.001;
00369                         start_slope=true;
00370                         wait(3);
00371                         consigne=0;
00372                         slope_time=2.;
00373                         start_slope=true;
00374                         wait(3);
00375       
00376                     break; 
00377                     
00378                     case '2':
00379                         device.printf("profile de mouvement 2\r\n");                              
00380                     break; 
00381 
00382                      case '3':
00383                         device.printf("moving3 Robotic Axis 1\r\n");             
00384                     break;  
00385 
00386                     case '4':
00387                         device.printf("moving4 Robotic Axis 1\r\n");           
00388                     break; 
00389       
00390                     case '5':
00391                         device.printf("moving5 Robotic Axis 1\r\n");             
00392                     break; 
00393  
00394                     case '6':
00395                         device.printf("moving6 Robotic Axis 1\r\n"); 
00396                     break;
00397                     
00398                     case '7':
00399                         device.printf("moving7 Robotic Axis 1\r\n"); 
00400                     break;
00401                     
00402                     case '8':
00403                         device.printf("moving8 Robotic Axis 1\r\n"); 
00404                     break;  
00405                     
00406                     case '9':
00407                         device.printf("moving9 Robotic Axis 1\r\n"); 
00408                     break;
00409                     
00410                     case '0':
00411                         device.printf("moving7 Robotic Axis 1\r\n"); 
00412                     break;                 
00413                 }                   
00414                 }
00415                                                     
00416         }