Diff: main.cpp

Revision:

0:6aa00967963c

diff -r 000000000000 -r 6aa00967963c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 13 15:44:15 2017 +0000
@@ -0,0 +1,416 @@
+#include "mbed.h"
+#include "qeihw.h"
+#include "NVIC_set_all_priorities.h"
+// déclaration du hardware
+Serial pc(USBTX, USBRX);//utilisation de la liaison usb 
+QEIHW qei(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_4X, QEI_INVINX_NONE );
+Serial device(p9, p10);// tx, rx.//definition de la liaison rs232
+DigitalOut frein_moteur1(p15); //changement momentané pour le projet r2d de p5 a p15
+DigitalOut sens_rotation_moteur1(p13);  
+PwmOut PWM1(p21);
+ 
+// définition des timers.
+Timer timer1; 
+Timer timer2;
+
+// définition des sortie leds
+DigitalOut led1(LED1);
+DigitalOut led3(LED3);
+
+char c;
+int t ;
+
+float PI=3.141592653589793;
+
+//position codeur en pulses 2000pulses/rotation 
+int position_actuelle;//position du codeur
+int position_actuelle_n1;//position du codeur a n-1
+
+//données mecanique & moteur 
+float J=3.375*1e-5;//0.005;//inertie en kg.m²
+float K=0.05;//2;//constante de couple  N.m/v
+float R=5;//K;//resistance du moteur en Ohms
+float Umax=24;//tension max en volts
+
+//parametres reglage
+float dt_vit(5*1e-4);
+float Ki=10;//default value=10;
+float Kp;
+float Kv;
+float K_sw(K);
+float tau;
+float taud=(10*dt_vit);
+float r1=3;//default value=2.5;
+float r2=r1;//default value=2.5;
+
+//variables generateur de consigne///////////////////////////////////////////////////////////////////////////////////////////
+float Teta_debut, Teta_fin, Teta_diff;
+float delta_Teta;
+float consigne;//en pulse 
+float slope_time(1.);//en seconde
+bool start_slope(false);//flag debut de montee
+int ticks_total;
+int count_ticks;
+int count_slope(0);
+int count_max(0);
+
+
+//variables calcul de l'asservissement en vitesse et position 
+float Tetaco; //valeur de la consigne position en radians
+float Tetam;//valeur mesuree de la position en radians
+
+float dTetam;
+float integ;
+float A;
+
+float dTetam_n1;
+float Tetaco_n1;
+float Tetam_n1;
+float integ_n1;
+float cyclic;
+int signe_rot;
+
+//declaration des differantes taches
+void task1_switch(void);
+void task2_switch(void);
+void task3_switch(void);
+void task4_switch(void);
+void task5_switch(void);
+void task6_switch(void);
+void task7_switch(void);
+void task8_switch(void);
+
+//declaration des differantes interuption timer
+Ticker time_up1; //define a Ticker, with name “time_up1”
+Ticker time_up2; //define a Ticker, with name “time_up2”
+Ticker time_up3; //define a Ticker, with name “time_up3”
+Ticker time_up4; //define a Ticker, with name “time_up4”
+Ticker time_up5; //define a Ticker, with name “time_up5”
+Ticker time_up6; //define a Ticker, with name “time_up6”
+Ticker time_up7; //define a Ticker, with name “time_up7”
+Ticker time_up8; //define a Ticker, with name “time_up8”
+
+//declaration des leds visuelle utiliser pour mesurer le temps des taches 
+DigitalOut myled1(LED1);
+DigitalOut myled3(LED3);
+
+////////////////////////////////////////
+//    Convertion pulses to radians    //
+////////////////////////////////////////
+
+//PI =3.141592653589793 =1000 pulses codeurs
+float pulsesToRadians(int pulses) 
+{
+  float radians_VAL;
+  radians_VAL=(pulses*PI)/1000.;
+  return radians_VAL;
+};
+////////////////////////////////////////
+//   calcule de la vitesse angulaire  //
+////////////////////////////////////////
+
+// ici le code du calcule 
+
+// Convert ASCII string to unsigned 32-bit decimal "string is null-terminated"
+  unsigned long Str2UDec(unsigned char string[]){
+  unsigned long i = 0;  // index
+  unsigned long n = 0;  // number
+  while(string[i] != 0){
+    n = 10*n +(string[i]-0x30);
+    i++;
+  }
+  return n;
+}
+
+////////////////////////////////////////
+//                TASKS1              //
+////////////////////////////////////////
+
+void task1_switch()
+{ 
+myled1=1;
+//lecture valeur codeur et conversion en radians
+position_actuelle=qei.GetPosition();//lecture valeur codeur et affectation a la variable globale 
+Tetam=pulsesToRadians(position_actuelle);//conversion valeur impulsionel en radians 
+
+//Calcul de la nouvelle consigne:
+
+//Etape1:derivee filtree 
+dTetam=1./(1.+taud*2./dt_vit)*(-dTetam_n1*(1.-taud*2./dt_vit)+2./dt_vit*(Tetam-Tetam_n1));
+
+//Etape2:calcul de integ non saturee
+integ=integ_n1+dt_vit/2.*((Tetaco-Tetam)+(Tetaco_n1-Tetam_n1));
+
+//Etape3:Calcul de A non saturee
+A=Kv*K_sw/Umax*(-dTetam+Kp*Ki*integ-Kp*Tetam);
+
+//Etape 4 et 5 : calcul de integ saturee
+if (A>1)
+    {
+    integ=1./Kp/Ki*(Umax/Kv/K_sw+dTetam+Kp*Tetam);
+    A=1;
+    }
+if(A<-1)
+    {
+    integ=1./Kp/Ki*(-Umax/Kv/K_sw+dTetam+Kp*Tetam); 
+    A=-1; 
+    }
+    
+//Etape 6:affectation du signe de rotation a l'etage de puissance 
+if (A>0)
+{
+    signe_rot=1;
+    sens_rotation_moteur1.write(signe_rot); //affectation du sens de rotation moteur1
+    cyclic=A;
+}
+else 
+{
+    signe_rot=0;
+    sens_rotation_moteur1.write(signe_rot); //affectation du sens de rotation moteur1
+    cyclic=-A;//peut etre une erreur ici sy??
+}
+
+PWM1.write(cyclic);// affectation  de la valeur calculé en pwm
+
+//   enregistrement des valeurs N-1
+position_actuelle_n1=position_actuelle;   
+dTetam_n1=dTetam;
+Tetaco_n1=Tetaco;
+Tetam_n1=Tetam;
+integ_n1=integ;
+
+myled1=0;
+//myled1=!myled1;//changement etat de la led1
+}
+
+////////////////////////////////////////
+//                TASKS2              //
+////////////////////////////////////////
+
+void task2_switch()
+{
+//        pc.printf("%f\r\n", Tetaco);//valeur float consigne  en radians le renvoyé par usb
+//        pc.printf("%i\r\n", position_actuelle);//valeur int du codeur renvoyé par usb        
+//        pc.printf("%f\r\n", Tetam);//valeur float du codeur en radians renvoyé par usb
+//        pc.printf("dTetam : %f\r\n", dTetam);//valeur float dTetam
+//        pc.printf("integ : %f\r\n", integ);//valeur float integ               
+//        pc.printf("%f\r\n", A);//valeur float du codeur en radians le renvoyé par usb
+//        pc.printf("%e\r\n", cyclic);//valeur float du codeur en radians le renvoyé par usb
+//        pc.printf("\r\n");//retour chario
+//        pc.printf("$%d %d %d;", position_actuelle,position_actuelle/2,position_actuelle/10 ); //utiliser avec logicielle serial port ploter 
+pc.printf("$%d;", position_actuelle); //utiliser avec logicielle serial port ploter 
+                 
+};
+
+////////////////////////////////////////
+//                TASKS3              //
+////////////////////////////////////////
+
+void task3_switch()
+{ 
+myled3=1;
+
+//generation de la tragectoire 
+if(start_slope)
+{
+    Teta_debut=Tetam;//affectation de la consigne a la variable globale
+    Teta_fin=pulsesToRadians(consigne);//affectation de la consigne a la variable globale
+    Teta_diff = Teta_fin - Teta_debut;
+    delta_Teta=(Teta_diff/slope_time)*5*1e-4;
+    //timer1.start();// déclenchement du timer1
+    count_slope = 0;
+    count_max = slope_time/(5*1e-4);
+    start_slope=false;
+}
+//float timeco = timer1.read();
+count_slope++;
+if (count_slope<=count_max){Tetaco=Tetaco+delta_Teta;}
+//if (timeco<slope_time){Tetaco=Tetaco+delta_Teta;}
+//if (timeco==slope_time){Tetaco=Teta_fin;}
+//if (timeco>slope_time){timer1.reset(); // remise à zéro du timer1
+
+//myled3=!myled3;
+myled3=0; 
+}
+
+////////////////////////////////////////
+//                TASKS4              //
+////////////////////////////////////////
+
+void task4_switch()
+{ 
+device.printf("$%d;", position_actuelle); //utiliser avec logicielle serial port ploter 
+
+}
+
+////////////////////////////////////////
+//                TASKS5              //
+////////////////////////////////////////
+
+void task5_switch()
+{
+// ici code de la tache 5
+}
+
+////////////////////////////////////////
+//                TASKS6              //
+////////////////////////////////////////
+
+void task6_switch()
+{ 
+// ici code de la tache 6
+}
+
+////////////////////////////////////////
+//                TASKS7              //
+////////////////////////////////////////
+
+void task7_switch()
+{ 
+// ici code de la tache 7
+}
+
+////////////////////////////////////////
+//                TASKS8              //
+////////////////////////////////////////
+
+void task8_switch()
+{ 
+// ici code de la tache 8
+}
+
+/////////////////////////////////////////////////////////////////////////////
+//                   declaration de la fonction moteur1                    //
+/////////////////////////////////////////////////////////////////////////////
+
+void motor1_mouve(int position_final,int T1)
+{
+//calcule nombres de ticks tache2 suite au temps T1 demander
+ 
+//calcule delta_Teta
+
+
+///////////////////////////////////////////////////
+//                debut du mouvement             //
+///////////////////////////////////////////////////
+
+timer1.start(); // déclenchement du timer1 
+timer2.start(); // déclenchement du timer2
+    
+//generation de consigne position   
+for(int i=0;i<=consigne;i++)
+{
+}
+    if (timer1.read_us()>T1) // lecture du temps du timer1 en us
+    { 
+      device.printf("TIME out  motor1  0\r\n"); 
+    } 
+
+    if (timer2.read_ms()>T1) // lecture du temps du timer1 en ms
+    { 
+      device.printf("TIME out  motor1  0\r\n"); 
+    }      
+timer1.reset(); // remise à zéro du timer1
+timer2.reset(); // remise à zéro du timer2
+};
+
+////////////////////////////////////////////////////////////////////////////
+//                       PROGRAMME PRINCIPAL                              //
+////////////////////////////////////////////////////////////////////////////
+                                                
+int main()
+            {
+               int cycles;
+                //NVIC_SetPriority(EINT3_IRQn, 252);//set interupt to highest priority 0.
+                //NVIC_SetPriority(TIMER1_IRQn, 253);// set mbed tickers to lower priority than other things
+                //NVIC_SetPriority(TIMER2_IRQn, 254);// set mbed tickers to lower priority than other things
+                //NVIC_SetPriority(TIMER3_IRQn, 255);// set mbed tickers to lower priority than other things
+                //NVIC_set_all_irq_priorities(0);
+                
+                qei.SetDigiFilter(480UL);//filtre 
+                qei.SetMaxPosition(0xFFFFFFFF);//position max 4294967295 pulses
+                
+//initialisation de la com rs232                
+                device.baud(9600);//rs232 28800 baud    
+                device.printf("serial rs232 ok \n");
+                
+                //calculs  de tau ,Kv,Kp          
+                tau=(J*R)/K;
+                Kv=r1*r2*tau*Ki-1.;
+                if(Kv<0){Kv = -Kv;}
+                Kp=r1*Ki*(1.+Kv)/Kv;
+                              
+//initialisation moteur   
+PWM1.period(0.00005);// initialisation du rapport cyclique fixe la période à 50us----f=20Khz
+frein_moteur1.write(0);//affectation de la valeur du frein moteur1
+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
+
+//lancement des tasks               
+                time_up1.attach(&task1_switch, 0.0005); //initialises the ticker  2Khz "tous les 500us il y as une interruption task1"
+                time_up2.attach(&task2_switch, 0.01); //initialises the ticker 100hz
+                time_up3.attach(&task3_switch, 0.0005); //initialises the ticker 2khz
+                time_up4.attach(&task4_switch, 0.01); //initialises the ticker 100hz
+                time_up5.attach(&task5_switch, 0.0005); //initialises the ticker 2kh
+                time_up6.attach(&task6_switch, 0.0005); //initialises the ticker 2kh
+                time_up7.attach(&task7_switch, 0.0005); //initialises the ticker 2kh
+                time_up8.attach(&task8_switch, 0.0005); //initialises the ticker 2kh
+ 
+count_ticks=0;                      
+        while(1)
+                {
+                 c='1';  
+               //device.scanf("%c",&c);//capture du caract ascii 
+                     
+                switch(c)
+                {
+                    case '1':
+                        consigne=70000;                                  
+                        slope_time=0.001;
+                        start_slope=true;
+                        wait(3);
+                        consigne=0;
+                        slope_time=2.;
+                        start_slope=true;
+                        wait(3);
+      
+                    break; 
+                    
+                    case '2':
+                        device.printf("profile de mouvement 2\r\n");                              
+                    break; 
+
+                     case '3':
+                        device.printf("moving3 Robotic Axis 1\r\n");             
+                    break;  
+
+                    case '4':
+                        device.printf("moving4 Robotic Axis 1\r\n");           
+                    break; 
+      
+                    case '5':
+                        device.printf("moving5 Robotic Axis 1\r\n");             
+                    break; 
+ 
+                    case '6':
+                        device.printf("moving6 Robotic Axis 1\r\n"); 
+                    break;
+                    
+                    case '7':
+                        device.printf("moving7 Robotic Axis 1\r\n"); 
+                    break;
+                    
+                    case '8':
+                        device.printf("moving8 Robotic Axis 1\r\n"); 
+                    break;  
+                    
+                    case '9':
+                        device.printf("moving9 Robotic Axis 1\r\n"); 
+                    break;
+                    
+                    case '0':
+                        device.printf("moving7 Robotic Axis 1\r\n"); 
+                    break;                 
+                }                   
+                }
+                                                    
+        }