File content as of revision 25:869b1c1f51a7:

/* #include */
#include "pins.h"

/* #define & constantes */
#define VMAXROT 0.040 // 0.020
#define VMAXLIN 0.100 // 0.060

/* Choix côté */

// Côté bleu
#define X_INIT 110
#define Y_INIT 1070
#define O_INIT 0


/*
// Côté jaune
#define X_INIT 2890
#define Y_INIT 1070
#define O_INIT 3.1414
*/


#define entraxe 253 // (Valeur théorique = 255)
const double coeffG = 0.16008537; // mm/tic
const double coeffD = 0.16059957; // mm/tic
/* COEFF ROTATION */
const double coeffPro = 0.040; // 0.020
const double coeffDer = 0.030; // 0.010
/* COEFF LIGNE DROITE */
const double coeffProDist = 0.003; // 0.0008
const double coeffDerDist = 0.003; // 0.0008

#define ACC_ROT 0.002
#define ACC_LIN 0.002

/* Stratégie */
int indiceStrategie = 0;

/* // Stratégie ultra simple
int objEtape[NbObj] = {0,1,1,1};
double objX[NbObj] = {0,660, 660, 210};
double objY[NbObj] = {0,1070,1650,1300};
int objRecule[NbObj]= {0,0,0,0};*/


 // Stratégie Lucas Blue
int objEtape[NbObj] = {0,1,1,1,1,1,1,1,1,1};
double objX[NbObj] = {110,200,660,627,450,235,698,300,300,230};
double objY[NbObj] = {1070,1070,1200,743,920,1124,672,460,1100,750};
int objRecule[NbObj]= {0,0,0,0,0,0,1,0,0,1}; 

/*// Stratégie Lucas Yellow
int objEtape[NbObj] = {0,1,1,1,1,1,1,1,1,1};
double objX[NbObj] = {2890,2800,2340,2373,2550,2765,2302,2700,2700,2800};
double objY[NbObj] = {1070,1070,1200,743,920,1124,672,460,1100,750};
int objRecule[NbObj]= {0,0,0,0,0,0,1,0,0,1};
*/

/* Variable globale */
Ticker Ticker_asserv; // Ticker pour l'asservissement en position

int action = 0; // Actions possibles : Rotation & Avancer/Reculer
long comptG = 0, comptD = 0; // Nb de tics compté par roue
double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
double x = X_INIT, y = Y_INIT, O = O_INIT;

/* Interruptions codeurs */

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

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




void odo2()
{
    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;
}
//*/


double distanceCible = 0;
double xC = 0, yC = 0; // x = xR et y = yR
double consigneOrientation = 0;
int signe = 1;
double cmdD = 0, cmdG = 0;
double erreurAngle = 0;
double erreurPre = 0;
double deltaErreur = 0;


// Ligne droite
double erreurPreDist = 0;
double deltaErreurDist = 0;


// NEW NEW NEW NEW

bool acc = 1;
double distancePrecedente = 0;
bool stt = 0; // Dépassement du point

double OrientationMem = 0;
double OrPlusPi2 = 0, OrMoinsPi2 = 0;

// Controle dépassement cible
double distanceMem = 0;
double distancePlus = 0;

void asserv()
{
    // Odométrie
    odo2();

    // Calcul de la cible

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

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

    if (xC >= x)
        consigneOrientation = signe * acos((abs(xC-x))/distanceCible);

    else
        consigneOrientation = signe * (3.1415 - acos((abs(xC-x))/distanceCible));

    // Switch de sélection de l'étape

    switch (action) {
        case 0: // Stand-by
            mot_dis();
            break;

        case 1: // Rotation
            mot_en();

            // Si on doit reculer
            if (objRecule[indiceStrategie]==1) {
                consigneOrientation += 3.1415;

                if(consigneOrientation>3.1415) consigneOrientation-=2*3.1415;
                if(consigneOrientation<-3.1415) consigneOrientation+=2*3.1415;
            }

            // Choix du sens de rotation

            double Osens = 0;
            if (O<0) Osens = O + (2.0f*3.1415f);
            else Osens = O;

            double consigneSens = 0;
            if (consigneOrientation<0) consigneSens = consigneOrientation + (2.0f*3.1415f);
            else consigneSens = consigneOrientation;

            double choixSens = consigneSens - Osens;

            if ((choixSens > 0) && (choixSens <= 3.1415) || (choixSens<(-3.1415))) {
                motGauche_bck();
                motDroite_fwd();
            } else {
                motGauche_fwd();
                motDroite_bck();
            }

            // Asservissement en position angulaire
            erreurAngle =  consigneOrientation - O;

            deltaErreur = erreurAngle - erreurPre;

            erreurPre  = erreurAngle;

            double deltaCommande = (abs(coeffPro * erreurAngle) + abs(coeffDer * deltaErreur));

            if(acc) {
                cmdG = cmdG + ACC_ROT; // +0.0008
                cmdD = cmdG;

                if (cmdG >= VMAXROT) acc = 0;
            } else {
                //acc = 0;

                if (deltaCommande < VMAXROT) {
                    cmdG = deltaCommande;
                    cmdD = cmdG;
                } else {
                    cmdG = VMAXROT;
                    cmdD = cmdG;
                }
            }

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

            if (O > (consigneOrientation - (1*0.0174533)) && O < (consigneOrientation + (1*0.0174533))) {
                action++;
                us_rbt_restart = 1;
                acc = 1;
                //azerty();
                distanceMem = distanceCible;
                distancePlus = 0;
                break;
            }

            break;

        case 2: // Avancer

            if (us_rbt_restart == 1 && us_libre == 1) {
                cmdG = 0;
                cmdD = 0;
                us_rbt_restart = 0;
                acc=1;
                mot_en();
            }

            deltaErreurDist = distanceCible - erreurPreDist;

            erreurPreDist  = distanceCible;

            double deltaCommande2 = (abs(coeffProDist * distanceCible) + abs(coeffDerDist * deltaErreurDist));

            if(acc) {
                cmdG = cmdG + ACC_LIN; // +0.0006
                cmdD = cmdG;

                if (cmdG >= VMAXLIN) {
                    acc = 0;
                }
            } else {

                if (deltaCommande2 < VMAXLIN) {
                    cmdG = deltaCommande2;
                    cmdD = cmdG;
                } else {
                    cmdG = VMAXLIN;
                    cmdD = cmdG;
                }
            }

            distancePlus += abs(dDist);

            if ((distanceCible < 10) || (distancePlus >= distanceMem)) {
                acc = 1;
                indiceStrategie++;

                if (indiceStrategie >= (int)NbObj) {
                    action = 0;
                } else {
                    action = objEtape[indiceStrategie];
                    xC = objX[indiceStrategie];
                    yC = objY[indiceStrategie];
                }

                distancePrecedente = 0;
                break;
            }

            if (objRecule[indiceStrategie] == 0) {
                motGauche_fwd();
                motDroite_fwd();
            } else {
                motGauche_bck();
                motDroite_bck();
            }

            vitesseMotG(cmdG);
            vitesseMotD(cmdD);

            distancePrecedente = distanceCible;
            break;


        default:
            mot_dis();
    }
}