Diff: main.cpp

Revision:

0:b07e06edae6f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 23 18:07:18 2011 +0000
@@ -0,0 +1,468 @@
+#include "mbed.h"
+#include "QEI.h"
+
+
+//***********************************************************//
+// #define _GAUCHE_ Control le moteur Gauche 
+// #define _DROIT_  Control le moteur Gauche 
+
+#define _DROIT_
+
+#ifdef _GAUCHE_
+    //*******************//
+    //   MOTEUR GAUCHE   //
+    //*******************//
+    
+    #define CAN_ID_ECRITURE_ASSERVISSEMENT  0x050
+    #define CAN_ID_INFO_ASSERVISSEMENT      0x110
+    #define CAN_ID_INFO_HACHEUR             0x101
+    
+    #define RECULE 0x01
+    #define AVANCE 0x02 
+    #define PARAMETRE_MOTEUR 1  
+    
+#endif
+
+#ifdef _DROIT_
+    //******************//
+    //   MOTEUR DROIT   //
+    //******************//
+
+    #define CAN_ID_ECRITURE_ASSERVISSEMENT  0x060
+    #define CAN_ID_INFO_ASSERVISSEMENT      0x210
+    #define CAN_ID_INFO_HACHEUR             0x201
+    
+    #define RECULE 0x01
+    #define AVANCE 0x02 
+    #define PARAMETRE_MOTEUR -1
+    
+#endif
+//***********************************************************//
+
+
+#define T_Time              0.5
+#define IMP_PAR_TOUR        36864           //impulsion par tour, codeur QEI
+#define DIAMETRE_ROUE       15              //diametre des roues en mm
+#define PI                  3.14159
+
+#define SEUIL_POSITION      0.3
+#define Kp_position         1
+#define Kd_position         0.5
+#define Ki_position         1
+#define MAX_INTEG_POSITION  4 //8
+
+#define VITESSE_MAX_ASSERVISSEMENT 120
+#define Kp_vitesse          2
+
+
+DigitalOut led1(LED1),led2(LED2),led3(LED3),led4(LED4);
+
+//defintion du port I2C
+//I2C i2c_D(p28, p27);
+//I2C i2c_G(p9, p10);
+
+//QEI qei(pinA,pinB,index,puls/tour)
+//QEI qei_D(p26,p25,p24,1);
+//QEI qei_G(p23,p22,p21,1);
+
+//liason serie avec le PC
+Serial pc(USBTX, USBRX);
+
+
+//definition de la class Moteur   
+class Moteur {
+public :
+
+    Moteur(PinName qeiA, PinName qeiB, PinName qei_index, int pulsesPerRev, PinName sda, PinName scl, PinName rd, PinName td);
+    
+    
+    //*********************//
+    //   CALCUL DISTANCE   //
+    //*********************// 
+    float Calcul_Distance(void) {
+        if (pulse != qei.getPulses()) 
+        {
+            pulse = PARAMETRE_MOTEUR * qei.getPulses();
+            distance = (pulse * PI * DIAMETRE_ROUE) / IMP_PAR_TOUR;
+            //pc.printf("\r\n  Distance = %f.2  ", distance);
+        }
+       return distance;
+    }
+
+
+    //********************//
+    //   CALCUL VITESSE   //
+    //********************//      
+    void Calcul_Vitesse(void){
+        
+        dist_1 = Calcul_Distance();
+        wait(0.01);
+        dist_2 = Calcul_Distance();
+        
+        Vitesse_moteur = (dist_2 - dist_1)*100;
+        //pc.printf("\r\n  Vitesse = %f.1  ", Vitesse_mesure);
+    }
+    
+    
+    //***************************//
+    //   CALCUL ERREUR VITESSE   //
+    //***************************//
+        
+    void Calcul_Erreur_Vitesse(int Vitesse_voulu)
+    {
+        
+        Last_Erreur_vitesse = Delta_Erreur_vitesse ; 
+        Calcul_Vitesse();
+        
+        if (Vitesse_moteur < 0 ) 
+            Vitesse_moteur = Vitesse_moteur * (-1);
+            
+        Erreur_vitesse = Vitesse_voulu - Vitesse_moteur;
+        
+    } 
+    
+    //****************************//
+    //   ASSERVISSEMENT VITESSE   //
+    //****************************//
+    void Asservissement_vitesse ( char DIRECTION ,char VITESSE , char VITESSE_MAX ,char ACCELERATION)
+    {   
+        
+        char result;
+        Calcul_Vitesse();
+        Vitesse_Sortie = Kp_vitesse * VITESSE ;
+        
+        //Bridage du moteur
+        if (Vitesse_Sortie > VITESSE_MAX_ASSERVISSEMENT) 
+            Vitesse_Sortie = VITESSE_MAX_ASSERVISSEMENT;
+        
+        if (Vitesse_Sortie < 0) 
+            Vitesse_Sortie = 0;
+        
+        data_speed[1] = Vitesse_Sortie;
+        data_comma[1] = DIRECTION;
+        data_accel[1] = ACCELERATION;
+        
+        /*
+        if (DIRECTION==0x00)
+        {
+            data_speed[1] = 0x44;
+            data_comma[1] = 0x00;
+            data_accel[1] = 0x00;
+        }
+        else
+        {
+            //Copie des valeur dans les tableaux de registre
+            data_speed[1] = Vitesse_Sortie;
+            data_comma[1] = DIRECTION;
+            data_accel[1] = ACCELERATION;
+        }
+        */
+        /*
+        if (Last_data_comma != data_comma[1])
+        {
+            //pc.printf("\r\n ASSERVISSEMENT  DIRECTION");
+            i2c.write(0xB0, data_comma,2,1);
+            Last_data_comma = data_comma[1];
+        }
+        
+        if (Last_data_accel != data_accel[1])
+        {
+            //pc.printf("\r\n ASSERVISSEMENT  ACCELERATION");
+            i2c.write(0xB0, data_accel,2,1);
+            Last_data_accel = data_accel[1];
+        }
+        
+        if (Last_data_speed != data_speed[1])
+        {
+            //pc.printf("\r\n ASSERVISSEMENT  VITESSE");
+            i2c.write(0xB0, data_speed,2,1);
+            Last_data_speed = data_speed[1];
+        }
+        */
+        
+        pc.printf("\r\n ASSERVISSEMENT  DIRECTION");
+        //while(i2c.write(0xB0, data_comma,2,1));
+        result=i2c.write(0xB0, data_comma,2,1);
+        //while(i2c.write(0xB0, data_accel,2,1));
+        pc.printf("\r\n ASSERVISSEMENT  VITESSE %d",result);
+        result=i2c.write(0xB0, data_speed,2,1);
+        //while(i2c.write(0xB0, data_speed,2,1));
+        pc.printf("\r\n ASSERVISSEMENT  VITESSE fin %d",result);
+        
+    }
+        
+    //****************************//
+    //   CALCUL ERREUR POSITION   //
+    //****************************//    
+    void Calcul_Erreur_Position(int Position_voulu)
+    {   
+        Last_Erreur_position = Erreur_position;
+        Position_moteur = Calcul_Distance();
+        Erreur_position = Position_voulu - Position_moteur;
+        
+        Delta_Erreur_position += 5 * Erreur_position ;
+
+        if (Delta_Erreur_position > MAX_INTEG_POSITION) 
+            Delta_Erreur_position = MAX_INTEG_POSITION;
+            
+        if (Delta_Erreur_position < - MAX_INTEG_POSITION) 
+            Delta_Erreur_position = - MAX_INTEG_POSITION; 
+    } 
+    
+    //********************************//
+    //   ASSERVISSEMENT POSITION BIS  //
+    //********************************//
+    void Asservissement_position()
+    {
+        Calcul_Erreur_Position(POSITION);
+        Speed = Kp_position * Erreur_position - Kd_position * Last_Erreur_position + Ki_position * Delta_Erreur_position;
+        
+        if (Speed < 0 )
+            Speed = Speed * (-1);
+            
+        if (Erreur_position < - SEUIL_POSITION )
+        {
+            
+            led1=0;
+            led2=1;
+            led3=0;
+            led4=0;
+           
+            Sens = RECULE;       
+        }    
+        else if (Erreur_position > SEUIL_POSITION )
+        {
+            
+            led1=0;
+            led2=0;
+            led3=1;
+            led4=0;
+            
+            Sens = AVANCE;   
+        }    
+        else 
+        {
+            
+            led1=1;
+            led2=0;
+            led3=0;
+            led4=1;
+            
+            Sens = 0x00;         
+        }    
+        
+        if (Speed > VITESSE*2 ) 
+            Speed = VITESSE*2;
+        
+        if (Speed > 254 ) 
+            Speed = 254;
+            
+        Asservissement_vitesse(Sens,Speed,VITESSE,ACCELERATION);
+    }
+    
+    //********************//
+    //   INITIALISATION   //
+    //********************//
+    void Initialisation (void)
+    {
+        //initialisation du CAN
+        can.frequency(1000000);
+        
+        i2c.frequency(100000);
+        
+        //Initialisation des param&#65533;tre d'asservissement.
+        ACCELERATION    = 0x00; 
+        VITESSE         = 0x0;
+        POSITION        = 0x0000;
+        
+        //initialisation des registres d'adresse
+        data_comma[0]   = 0x00;
+        data_speed[0]   = 0x02;
+        data_accel[0]   = 0x03;
+    }
+
+
+    //******************************//
+    //   DEBUG SERIAL I2C HACHEUR   //
+    //******************************//    
+    void debug_serial_i2c_hacheur (void)
+    {
+        char data_recu[8];
+        unsigned char i;
+        
+        i2c.read(0xB0, data_recu ,8);
+        
+        pc.printf("\r\n \r\n");
+        pc.printf("temp moto unus revi | comm stat spee acce\r\n"); 
+    
+        for(i=0;i<8;i++)
+        {
+            if (i==4) pc.printf("| ");
+            pc.printf(" %02x  ", data_recu[i] );
+        }    
+    }
+    
+    
+    //***********************//
+    //   DEBUG SERIAL mBed   //
+    //***********************//  
+    void debug_serial_mbed (void)
+    {
+        pc.printf(" %d     | %.1f    | %02x    |  %.lf  | %d | %.1f \r\n ",
+            data_speed[1], Vitesse_moteur, Sens,Position_moteur,Speed,Erreur_position);
+    }
+    
+    
+    //*****************************************//
+    //   DEBUG SERIAL I2C mBed ASSERVISSEMENT  //
+    //*****************************************//  
+    void debug_serial_i2c_mbed_asservissement(void)
+    {
+        pc.printf(" %d = %.lf - %.1f + %.lf  \r\n ",
+            Speed, Kp_position * Erreur_position , Kd_position * Last_Erreur_position , Ki_position * Delta_Erreur_position);
+    }
+    
+    //**********************//
+    //   CAN MESSAGE SEND   //
+    //**********************//
+    void CANMSG_Send(int CANID,char CAN_data[])
+    {
+        char i; 
+        CANMessage canmsg = CANMessage();                                  
+
+        canmsg.id        = CANID;        /*L'identification du message*/
+        canmsg.len       = 8;            /*Longeur de la trame en octet*/
+        canmsg.format    = CANStandard;  /*Format de la trame CANStandard 29 bits*/
+        canmsg.type      = CANData;      /*Type de la trame CANData*/
+        
+        for(i=0;i<8;i++)           
+            canmsg.data[i] = CAN_data[i];
+  
+        can.write(canmsg);                /*Envoi du message*/  
+    }
+    
+    //*****************************//
+    //   CAN INFO ASSERVISSEMENT   //
+    //*****************************//
+    void CAN_INFO_Asservissement (void)
+    {
+        char data_asservicement[8];  
+        short Position_moteur_Can, Vitesse_moteur_Can;
+        
+        Position_moteur_Can = Position_moteur;
+        Vitesse_moteur_Can = Vitesse_moteur;
+        
+        if (Vitesse_moteur_Can <0)
+            Vitesse_moteur_Can = Vitesse_moteur_Can *(-1);
+            
+        data_asservicement[0] = Sens;
+        data_asservicement[1] = Vitesse_moteur_Can & 0x00FF;
+        data_asservicement[2] = 0;
+        data_asservicement[3] = 0;
+        
+        data_asservicement[4] = POSITION >>8;
+        data_asservicement[5] = POSITION & 0x00FF;
+        data_asservicement[6] = Position_moteur_Can >>8;
+        data_asservicement[7] = Position_moteur_Can & 0x00FF;
+        
+        CANMSG_Send(CAN_ID_INFO_ASSERVISSEMENT,data_asservicement);
+    }
+    
+    //**********************//
+    //   CAN INFO Hacheur   //
+    //**********************//
+    void CAN_INFO_Hacheur (void)
+    {
+        char data_Hacheur[8];  
+        pc.printf("\r\n ENTRER INFO  ");
+        i2c.read(0xB0, data_Hacheur ,8);
+        pc.printf("\r\n SORTIE INFO  ");
+        //temp moto unus revi | comm stat spee acce
+        CANMSG_Send(CAN_ID_INFO_HACHEUR,data_Hacheur);
+    }
+    
+    //*************************//
+    //   CAN MESSAGE RECIEVE   //
+    //*************************//
+    void CAN_RecieveInterrupt_Routine(void)
+    {
+    
+        static CANMessage can_recieve_msg;
+                
+        if(can.read(can_recieve_msg))
+        {
+            if(can_recieve_msg.id==CAN_ID_ECRITURE_ASSERVISSEMENT)
+            {
+            
+                Sens            = can_recieve_msg.data[0] ; 
+                VITESSE         = can_recieve_msg.data[1] ; 
+                ACCELERATION    = can_recieve_msg.data[2] ;
+                POSITION = (short) can_recieve_msg.data[6]<<8 | can_recieve_msg.data[7];
+                 
+            }
+                                                
+        }      
+    }
+
+// protected bite:
+    QEI qei;
+    I2C i2c;
+    CAN can;
+    
+    short POSITION;
+    char VITESSE,ACCELERATION, Sens;
+    
+    float Position_moteur, Erreur_position, Delta_Erreur_position, Last_Erreur_position ;
+    float distance , dist_1, dist_2;
+    float Vitesse_Sortie, Vitesse_moteur, Erreur_vitesse,Last_Erreur_vitesse,Delta_Erreur_vitesse;
+    
+    int pulse,i;
+    int Speed;
+    
+    char Last_data_speed, Last_data_comma, Last_data_accel;
+    
+    // data_hacheur[2]={ registre, valeur}
+    char data_comma[2];
+    char data_speed[2];
+    char data_accel[2];
+
+};
+
+//constructeur de la class "Moteur"
+Moteur::Moteur(PinName qeiA, PinName qeiB, PinName qei_index, int pulsesPerRev, PinName sda, PinName scl, PinName rd, PinName td) :
+        qei(qeiA, qeiB, qei_index, pulsesPerRev),
+        i2c(sda,scl), 
+        can(rd,td){
+    distance = 0.0;
+    pulse  = 0;
+}
+
+//definiton des Classes
+Moteur moteur(p21,p22,p23,1,p9,p10,p30,p29);//, moteur_G(p23,p22,p21,1,p9,p10);
+
+
+//fonction principal
+int main()
+{
+    moteur.Initialisation();
+    pc.printf("\r\n DEBUT");
+        
+    while (1) 
+    {
+        
+        
+        pc.printf("\r\n ASSERVISSEMENT");
+        moteur.CAN_INFO_Asservissement();
+        //moteur.CAN_INFO_Hacheur();
+         pc.printf("\r\n RECEIVE");
+        moteur.CAN_RecieveInterrupt_Routine();
+         pc.printf("\r\n POSITION");                            
+        moteur.Asservissement_position();
+        //moteur.debug_serial_mbed();
+        //moteur.debug_serial_i2c_mbed_asservissement();
+        
+
+         pc.printf("\r\n TEMPO");
+        wait(0.01);
+    }
+}
\ No newline at end of file