File content as of revision 16:ae65ce77b1f9:

//Nom du fichier : odo_asserv.cpp
#include "pins.h"

#define VMAX 20

///// VARIABLES
Ticker ticker_odo;
Ticker ticker_asserv;

// Coeff à définir empiriquement
const double coeffGLong = 5.956, coeffDLong = 5.956; // constantes permettant la transformation tic/millimètre
const double coeffGAngl = 737.447, coeffDAngl = 748.057; // constantes permettant la transformation tic/radian

long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur


///// INTERRUPTIONS CODEURS

void cdgaRise()
{
    if(cdgB) comptG--;
    else comptG++;
}

void cddaRise()
{
    if(cddB) comptD--;
    else comptD++;
}

/*
// odo1()
double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
double x = 0, y = 0, O = 0;


void odo1()
{
    dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2;
    dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl));

    x += dDist * cos(dAngl);
    y += dDist * sin(dAngl);
    O += dAngl;

    comptG = 0;
    comptD = 0;
}
*/

///*
// odo2()
//#define diametre 51.45 // 51.45 théorique
//#define N 1000 // 1000 théorique
#define entraxe 253 // 255 théorique
//const double coeffG = ((double)(diametre/2)/(double)N)*2.0f*3.1415f;
//const double coeffD = ((double)(diametre/2)/(double)N)*2.0f*3.1415f;
const double coeffG = 0.16008537;
const double coeffD = 0.16059957;
double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
double x = 0, y = 0, O = 0;

double vitG = 0, vitD = 0;
#define tpsTicker 0.020f

void odo2()
{
    vitG = (double) ((comptG * coeffG) / tpsTicker);
    vitD = (double) ((comptD * coeffD) / tpsTicker);

    dDist = (double) ((comptG * coeffG) + (comptD * coeffD)) / 2.0f;
    dAngl = (double) ((comptD * coeffD) - (comptG * coeffG)) / entraxe;

    x += (double) dDist * cos(O);
    y += (double) dDist * sin(O);
    O += (double) dAngl;

    if (O > 3.1415) O = O - (2.0f * 3.1415f);
    if (O < -3.1415) O = O + (2.0f * 3.1415f);

    comptG = 0;
    comptD = 0;
}
//*/

/*
// odo3()
#define diametre 51.45
#define N 1000
#define entraxe 255
const double coeffG = 1/5.956;
const double coeffD = 1/5.956;

void odo3()
{
    dDist = (double) ((comptG * coeffG) + (comptD * coeffD)) / 2.0f;
    dAngl = (double) ((comptD * coeffD) - (comptG * coeffG)) / entraxe;

    x += (double) dDist * cos(O);
    y += (double) dDist * sin(O);
    O += (double) dAngl;

    comptG = 0;
    comptD = 0;
}
*/

double distanceCible = 0;
double xC = 0, yC = -100; // x = xR et y = yR
double consigneOrientation = 0;
//double consigneOrientation = (90*3.1415)/180;
int signe = 1;
int cmdD = 0, cmdG = 0;
double erreurAngle = 0;
double erreurPre = 0;
double deltaErreur = 0;
const double coeffPro = 15.0; // 5.0 de base
const double coeffDer = 3.0;
bool phase_acc = 1;

void asserv()
{
    odo2();


    distanceCible = sqrt((xC-x)*(xC-x)+(yC-y)*(yC-y));

    /*
    if(y > yC) {
        signe = -1;
    } else {
        signe = 1;
    }

    consigneOrientation = signe * acos((xC-x)/((xC-x)*(xC-x)*(yC-y)*(yC-y)));
    */


    cmdD = abs((int)distanceCible);
    if(cmdD>VMAX) {
        cmdD = VMAX;
    }
    cmdG = cmdD;


    /*
    motGauche_fwd();
    motDroite_fwd();
    vitesseMotG(cmdG);
    vitesseMotD(cmdD);
    */

    erreurAngle =  consigneOrientation - O;

    deltaErreur = erreurAngle - erreurPre;

    erreurPre  = erreurAngle;

    int deltaCommande = coeffPro * erreurAngle + coeffDer * deltaErreur;

    motGauche_bck();
    motDroite_fwd();

    cmdG = 0;
    cmdD = 0;

    cmdG += deltaCommande;
    cmdD -= deltaCommande;

    if(cmdD>VMAX) {
        cmdD =VMAX;
    } else if(cmdD < -VMAX) {
        cmdD = -VMAX;
    }

    if(cmdG>VMAX) {
        cmdG =VMAX;
    } else if(cmdG < -VMAX) {
        cmdG = -VMAX;
    }

    /*
    if (cmdD > 0) {
        //motDroite_fwd();
        motDroite_bck();
    } else {
        //motDroite_bck();
        motDroite_fwd();
    }

    if (cmdG > 0) {
        motGauche_fwd();
        //motGauche_bck();
    } else {
        motGauche_bck();
        //motGauche_fwd();
    }
    */

    /*
    if(cmdD>150) {
        cmdD =150;
    } else if(cmdD < 0) {
        cmdD = 0;
    }

    if(cmdG>150) {
        cmdG = 150;
    } else if(cmdG < 0) {
        cmdG = 0;
    }
    */

    /*
    if (consigneOrientation - O < 3.1415) {
        motGauche_bck();
        motDroite_fwd();
    } else {
        motGauche_fwd();
        motDroite_bck();
    }
    */

    if (phase_acc) {
        cmdG += 1;
        cmdD = cmdG;

        if (cmdG >= VMAX) phase_acc = 0;
    }

    vitesseMotG(abs(cmdG));
    vitesseMotD(abs(cmdD));

    if (O > (consigneOrientation - (2*0.0174533)) && O < (consigneOrientation + (2*0.0174533))) mot_dis();
}