File content as of revision 8:91eb7435c3e0:

//Includes
#include "mbed.h"
#include "7366_lib.h"
#include "TLE5206_lib.h"

// Caractéristiques de structure
#define RAYON_ROUE 0.025 // [m]
#define DIST1 0.15 // [m]
#define DIST2 0.15 // [m]
#define DIST3 0.0075 // [m]
#define PI 3.14159265359
#define RESOLUTION_ENCO 14336 // = 14*1024 = rapport_reduction * nbr de top par tour
#define ETALONNAGE_LACET 0.88
#define ETALONNAGE_XY 1.065
// Liaison SPI avec les compteurs 
#define SPI_SCLK PA_5 //A4
#define SPI_MISO PA_6 //A5
#define SPI_MOSI PA_7 //A6
#define SPI_CS3 PA_0//A0
#define SPI_CS2 PA_1
#define SPI_CS1 PA_3

// Vers le pont en H
#define H1_IN1 PA_9  //D1 - PWM1/2
#define H1_IN2 PA_10 //D0 - PWM1/3
#define H2_IN1 PA_8  //D9 - PWM1/1
#define H2_IN2 PB_7  //D4 - PWM17/1
#define H3_IN1 PB_6  //D5 - PWM16/1
#define H3_IN2 PA_4  //A3 - PWM3/2

#define DECOUP_HACH 50 //us - 20 000 kHz pour les oreilles
#define PERIODE_AFF 500 //ms
#define PERIODE_ASSERV 50 //ms
#define Te 50 //ms /!\ Te doit être égale à la période d'asservissement

// Constantes Asservissement
#define GAIN_POS 1
#define GAIN_ANG 0
#define Ti 200 //ms
#define GAIN_POS_INT 0.1
#define GAIN_ANG_INT 0.1
#define ERREUR_POS 0.5
#define ERREUR_ANG 0.5

Serial pc(USBTX,USBRX);
Timer timer;
DigitalOut myled(LED3);

SPI_7366 compt1(SPI_MOSI, SPI_MISO, SPI_SCLK,SPI_CS1);
SPI_7366 compt2(SPI_MOSI, SPI_MISO, SPI_SCLK,SPI_CS2);
SPI_7366 compt3(SPI_MOSI, SPI_MISO, SPI_SCLK,SPI_CS3);

TLE5206 moteur1(H1_IN1,H1_IN2);
TLE5206 moteur2(H2_IN1,H2_IN2);
TLE5206 moteur3(H3_IN1,H3_IN2);

struct Vect3{
    double x;
    double y;
    double z;
};


struct Vect2{
    float x;
    float y;
};

Vect3 initVect3(){
    Vect3 result;
    result.x = 0;
    result.y = 0;
    result.z = 0;
    return result;
}

Vect3 calculatePosition(){
    static Vect3 theta = initVect3();
    static Vect3 position = initVect3();
    Vect3 dTheta;
    double dPsi = 0;
    
    dTheta.x = 2*PI/RESOLUTION_ENCO*compt2.read_value() - theta.x;
    dTheta.y = 2*PI/RESOLUTION_ENCO*compt1.read_value() - theta.y;
    dTheta.z = 2*PI/RESOLUTION_ENCO*compt3.read_value() - theta.z;
    theta.x += dTheta.x;
    theta.y += dTheta.y;
    theta.z += dTheta.z;
    
    dPsi = ETALONNAGE_LACET*RAYON_ROUE * ( dTheta.x + dTheta.y + dTheta.z)/(DIST1 + DIST2 + DIST3);

    position.z += dPsi;// Psi actuel
    
    position.x += ETALONNAGE_XY*((-RAYON_ROUE * dTheta.x + DIST1 * dPsi) *  cos(position.z) + (-RAYON_ROUE * dTheta.y + DIST1 * dPsi) *  cos(position.z + 2*PI/3) + (-RAYON_ROUE * dTheta.z + DIST1 * dPsi)*cos(position.z + 4*PI/3))*2/3;
    position.y +=-ETALONNAGE_XY*((-RAYON_ROUE * dTheta.x + DIST1 * dPsi) *  sin(position.z) + (-RAYON_ROUE * dTheta.y + DIST1 * dPsi) *  sin(position.z + 2*PI/3) + (-RAYON_ROUE * dTheta.z + DIST1 * dPsi)*sin(position.z  + 4*PI/3))*2/3;
    
    return position;
}

Vect3 calcErreur(Vect3 consigne, Vect3 position){
    Vect3 erreur;
    erreur.x = position.x - consigne.x;
    erreur.x = position.y - consigne.y;
    erreur.x = position.z - consigne.z;
    return erreur;
}

Vect3 calcCommandeXYZ(Vect3 erreur){
    static Vect3 commande;
    static Vect3 oldErreur;
    if (erreur.x > ERREUR_POS){
        commande.x = GAIN_POS*erreur.x;
    } else {
        commande.x = commande.x + GAIN_POS*(Ti+Te)/Te*erreur.x - GAIN_POS*oldErreur.x;
        //commande.x = GAIN_POS*(erreur.x + GAIN_POS_INT*commande.x);
    }
    if (erreur.y > ERREUR_POS){
        commande.y = GAIN_POS*erreur.y;
    } else {
        commande.y = commande.y + GAIN_POS*(Ti+Te)/Te*erreur.y - GAIN_POS*oldErreur.y;
        //commande.y = GAIN_POS*(erreur.y + GAIN_POS_INT*commande.y);
    }
    if (erreur.z > ERREUR_ANG){
        commande.z = GAIN_ANG*erreur.z;
    } else {
        commande.z = commande.z + GAIN_ANG*(Ti+Te)/Te*erreur.z - GAIN_ANG*oldErreur.z;
        //commande.z = GAIN_ANG*(erreur.z + GAIN_ANG_INT*commande.z);
    }
    oldErreur = erreur;
    return commande;
}

Vect3 calcCommande123(Vect3 commandeXYZ, Vect3 position){
    Vect3 commande123;
    commande123.x = -commandeXYZ.x*cos(position.z+0*PI/3)+commandeXYZ.y*sin(position.z+0*PI/3)+commandeXYZ.z;
    commande123.y = -commandeXYZ.x*cos(position.z+2*PI/3)+commandeXYZ.y*sin(position.z+2*PI/3)+commandeXYZ.z;
    commande123.z = -commandeXYZ.x*cos(position.z+4*PI/3)+commandeXYZ.y*sin(position.z+4*PI/3)+commandeXYZ.z;
    return commande123;
}

void moveBot(Vect3 commande123){
    moteur1.write(commande123.y); 
    moteur2.write(commande123.x);
    moteur3.write(commande123.z);  
}

int main(){
    
    //setup
    compt1.setup();
    compt2.setup();
    compt3.setup();
    
    moteur1.setup(DECOUP_HACH);
    moteur2.setup(DECOUP_HACH);
    moteur3.setup(DECOUP_HACH);
    
    timer.start();
    pc.printf("SETUP effectue\n\r");
    
    //variables
    Vect3 position    = initVect3();
    Vect3 erreur      = initVect3();
    Vect3 commandeXYZ = initVect3();
    Vect3 commande123 = initVect3();
    Vect3 consigne    = initVect3();
    consigne.x = 0.30;
    consigne.y = 0.30;
    consigne.z = PI/6;

    uint32_t seuilAffichage = PERIODE_AFF;
    uint32_t seuilAsserv = PERIODE_ASSERV;
    
    // Loop
    while(1) {
        
        if (timer.read_ms() > seuilAffichage){
            seuilAffichage += PERIODE_AFF;
            //pc.printf("lacet : %f\n\rpositionX : %f\n\rpositionY: %f\n\n\r",360/(2*PI)*position.z, position.x, position.y);
            pc.printf("erreur lacet : %f\n\rerreurX : %f\n\rereeurY: %f\n\n\r",erreur.z, erreur.x, erreur.y);
            pc.printf("commande lacet : %f\n\rcommandeX : %f\n\rcommandeY: %f\n\n\r",commande123.z, commande123.x, commande123.y);
            //pc.printf("compt3 : %f\n\rcompt1 : %f\n\rcompt2: %f\n\n\r",2*PI/RESOLUTION_ENCO*compt3.read_value(),2*PI/RESOLUTION_ENCO*compt1.read_value(), 2*PI/RESOLUTION_ENCO*compt2.read_value());
            //pc.printf("compt3 : %f\n\rcompt1 : %f\n\rcompt2: %f\n\n\r",2*PI/RESOLUTION_ENCO*compt3.read_value(),2*PI/RESOLUTION_ENCO*compt1.read_value(), 2*PI/RESOLUTION_ENCO*compt2.read_value());
            
            myled = !myled;
        }
        if (timer.read_ms() > seuilAsserv){
            seuilAsserv += PERIODE_ASSERV;
            position    = calculatePosition();
            erreur      = calcErreur(consigne, position);
            commandeXYZ = calcCommandeXYZ(erreur);
            commande123 = calcCommande123(commandeXYZ, position);
            moveBot(commande123);
        }        
    }
}