File content as of revision 0:6aa00967963c:

#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;                 
                }                   
                }
                                                    
        }