File content as of revision 6:ea6b30c4bb01:

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

///// VARIABLES
const double coeffGLong = 1, coeffDLong = 1; // constantes permettant la transformation tic/millimètre => à définir
const double coeffGAngl = 1, coeffDAngl = 1; // constantes permettant la transformation tic/degrés => à définir

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

double xR = 0, yR = 0; // position du robot
double xC = 0, yC = 0; // position de la cible du robot

double dDist = 0, dAngl = 0; // delta position et angle

double orientation = 0; // cap du robot
double consigneOrientation = 0; // angle entre le robot et la cible

int signe = 1; // variable permettant de savoir si la cible est à gauche ou à droite du robot

double distanceCible = 0; // distance entre le robot et la cible

double coeffP = 1, coeffD = 1; // paramètre de l'asservissement en angle du robot

double erreurAngle = 0, erreurPre = 0, deltaErreur = 0; // variables utilisées pour asservir le robot

float cmdG =0, cmdD=0; // Commandes à envoyer aux moteurs

/////

///// INTERRUPTIONS CODEURS

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

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

/////

///// ODOMETRIE

void mvtsRobot()
{
    // Calcul variations de position en distance et en angle
    dDist =(coeffGLong*comptG + coeffDLong*comptD)/2;
    dAngl = coeffDAngl*comptD - coeffGAngl*comptG;

    // Actualisation de la position du robot en xy et en orientation
    xR += dDist*cos(dAngl);
    yR += dDist*sin(dAngl);
    orientation += dAngl;

    // Calcul distance séparant le robot et la cible
    distanceCible = sqrt((xC-xR)*(xC-xR)+(yC-yR)*(yC-yR));

    // On regarde si la cible est à gauche ou à droite du robot
    if(yR > yC) {
        signe = 1; // Inversé, non ?
    } else {
        signe = -1;
    }

    // Calcul angle entre le robot et la cible
    consigneOrientation = signe * acos((xC-xR)/((xC-xR)*(xC-xR)*(yC-yR)*(yC-yR)));
    
    // On détermine les commandes à envoyer aux moteurs
    cmdD = abs((float)distanceCible);
    if(cmdD>255)
    {
        cmdD = 255;
    }
    cmdG = cmdD;
    
    // Calcul de l'erreur
    erreurAngle =  consigneOrientation - orientation;
                                       
    // Calcul de la différence entre l'erreur au coup précédent et l'erreur actuelle.
    deltaErreur = erreurAngle - erreurPre;

    // Mise en mémoire de l'erreur actuelle
    erreurPre  = erreurAngle;
                
    // Calcul de la valeur à envoyer aux moteurs pour tourner
    int deltaCommande = coeffP*erreurAngle + coeffD * deltaErreur;
                                       
    cmdG += deltaCommande;
    cmdD -= deltaCommande;
                                       
    if(cmdD>255)
    {
        cmdD = 255;
    }else if(cmdD < 0)
    {
        cmdD = 0;
    }
                                       
    if(cmdG>255)
    {
        cmdG = 255;
    }else if(cmdG < 0)
    {
        cmdG = 0;
    }
}