File content as of revision 20:7d206773f39e:

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

#define VMAXROT 0.050 // 0.030
#define VMAXLIN 0.130 // 0.050

//const float DISTLIM = 150.0;

int cptErreur = 0;

// Objectifs
//#define NbObj 5
int indice = 0;
int objEtape[NbObj] = {0,1,1,1,1,1,1,1};
double objX[NbObj] = {0,300,660,660,210,660,400,210};
double objY[NbObj] = {0,1070,1270,1650,1300,1650,1800,1300};
int objRecule[NbObj] = {0,0,0,0,0,1,0,0};

///// 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

//bool objOnce = 0;

///// INTERRUPTIONS CODEURS

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

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

///*
// 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 = 110, y = 1070, O = 0;

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;
//double consigneOrientation = (90*3.1415)/180;
int signe = 1;
double cmdD = 0, cmdG = 0;
double erreurAngle = 0;
double erreurPre = 0;
double deltaErreur = 0;
const double coeffPro = 0.075; // 0.023 de base
const double coeffDer = 0.060; // 0.023 de base

// Ligne droite
//double erreurDist = 0;
double erreurPreDist = 0;
double deltaErreurDist = 0;
const double coeffProDist = 0.0005; // 0.010 de base
const double coeffDerDist = 0.0005; // 0.010 de base

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

// Reculer
double distanceFaite = 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 (fnc) {
        case 0: // Stand-by
            mot_dis();
            break;

        case 1: // Rotation
            mot_en();

            // Si on doit reculer
            if (objRecule[indice]==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(objRecule[indice] == 0) {
            //if (choixSens > 3.1415) choixSens -= (2.0f * 3.1415f);
            //else if (choixSens < 3.1415) choixSens += (2.0f * 3.1415f);

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

            /*
            if(objRecule[indice] == 1) {
                //if (choixSens > 3.1415) choixSens -= (2.0f * 3.1415f);
                //else if (choixSens < 3.1415) choixSens += (2.0f * 3.1415f);

                if (choixSens > 0) {
                    motGauche_fwd();
                    motDroite_bck();
                } else {
                    motGauche_bck();
                    motDroite_fwd();
                }
            }
            */

            /*
            double choixSens = consigneOrientation - O;

            if(objRecule[indice] == 0) {
                if (choixSens > 3.1415) choixSens -= (2.0f * 3.1415f);
                else if (choixSens < 3.1415) choixSens += (2.0f * 3.1415f);

                if (choixSens > 0) {
                    motGauche_bck();
                    motDroite_fwd();
                } else {
                    motGauche_fwd();
                    motDroite_bck();
                }
            }

            if(objRecule[indice] == 1) {
                if (choixSens > 3.1415) choixSens -= (2.0f * 3.1415f);
                else if (choixSens < 3.1415) choixSens += (2.0f * 3.1415f);

                if (choixSens > 0) {
                    motGauche_fwd();
                    motDroite_bck();
                } else {
                    motGauche_bck();
                    motDroite_fwd();
                }
            }
            */

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

            deltaErreur = erreurAngle - erreurPre;

            erreurPre  = erreurAngle;

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

            if(acc) {
                cmdG = cmdG + 0.0007; // +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))) {
                //mot_dis();
                fnc++;
                rebooted = 1;
                acc = 1;
                //objOnce = 0;
                break;
            }

            break;

        case 2: // Avancer
        
            if (rebooted == 1 && wtt==1) {
                cmdG = 0;
                cmdD = 0;
                rebooted = 0;
                acc=1;
                mot_en();
            }

            /*
            if(objRecule[indice]==0) {
                if ((::distance[0] >= 120) && (::distance[1] >= 140) && (::distance[5] >= 140)) {
                    mot_en();
                    cptErreur=0;

                } else {
                    cptErreur++;

                    if(cptErreur>=5) {
                        mot_dis();
                        acc = 1;
                    }

                    break;
                }
            }

            else if(objRecule[indice]==1) {
                if ((::distance[2] >= 140) && (::distance[3] >= 120) && (::distance[4] >= 140)) {
                    mot_en();
                    cptErreur=0;

                } else {
                    cptErreur++;

                    if(cptErreur>=5) {
                        mot_dis();
                        acc = 1;
                    }

                    break;
                }
            }
            */

            /*
            cmdD = abs((int)distanceCible)*0.0001; // *0.005
            if(cmdD>VMAXLIN) {
                cmdD = VMAXLIN;
            }
            cmdG = cmdD;
            */

            // Asservissement distance à parcourir
            //erreurDist =  consigneOrientation - O;

            deltaErreurDist = distanceCible - erreurPreDist;

            erreurPreDist  = distanceCible;

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

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

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

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

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

            //if (distanceCible < 15)[
            //if ((x < xC+10) && (x > xC-10) && (y < yC+10) && (y > yC-10)) {
            if ((distanceCible < 10) || (acc==0 && wtt==1 && rebooted==0 && (distancePrecedente < distanceCible))) {
                //mot_dis();
                //fnc++;
                acc = 1;
                //stt = 0;

                //if (objOnce == 0) {
                indice++;
                //objOnce=1;



                //}

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

                distancePrecedente = 0;
                break;

                /*
                if (indice>=NbObj) {
                    fnc = 0;
                } else {
                    fnc = objEtape[indice];
                    xC = objX[indice];
                    yC = objY[indice];
                }
                */
            }

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

            vitesseMotG(cmdG);
            vitesseMotD(cmdD);

            distancePrecedente = distanceCible;
            break;

        /*
        case 3: // Reculer

                if (objRecule[indice] == 1) {
                    distanceFaite += abs(dDist);

                    motGauche_bck();
                    motDroite_bck();
                    vitesseMotG(0.01);
                    vitesseMotD(0.01);
                }

                if ((distanceFaite >= 100) || (objRecule[indice] == 0)) {
                    indice++;
                    fnc = objEtape[indice];
                    xC = objX[indice];
                    yC = objY[indice];
                    distanceFaite = 0;
                }

            break;
            */

        default:
            mot_dis();
    }
}