File content as of revision 0:b07e06edae6f:

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