odo_asserv.cpp

00001 //Nom du fichier : odo_asserv.cpp
00002 #include "pins.h"
00003 //#define _PI_ 3.14159265359
00004 
00005 ///// VARIABLES
00006 Ticker ticker_odo;
00007 
00008 // Coeff à définir empiriquement
00009 const double coeffGLong = 5.956, coeffDLong = -5.956; // constantes permettant la transformation tic/millimètre
00010 //const double coeffGAngl = 53791/(12*2*_PI_), coeffDAngl = 54972/(12*2*_PI_); // constantes permettant la transformation tic/radian
00011 const double coeffGAngl = 713.425, coeffDAngl = 729.089; // constantes permettant la transformation tic/radian
00012 
00013 long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur
00014 
00015 
00016 ///// INTERRUPTIONS CODEURS
00017 
00018 void cdgaRise()
00019 {
00020     if(cdgB) comptG++;
00021     else comptG--;
00022 }
00023 
00024 void cddaRise()
00025 {
00026     if(cddB) comptD++;
00027     else comptD--;
00028 }
00029 
00030 ///*
00031 ///// 1) ODOMÉTRIE Geonobot : Approximation par segment de droite
00032 
00033 #define entraxe 245 // Distance entre les roues codeuses
00034 double xB = 0, yB = 0, phiB = 0; // Nouvelles coordonnées
00035 double xA = 0, yA = 0, phiA = 0; // Dernières coordonnées calculées
00036 double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
00037 double distG = 0, distD = 0; // Distance parcourue par chaque roue
00038 
00039 void odoGeonobot1()
00040 {
00041     xA = xB;
00042     yA = yB;
00043     phiA = phiB;
00044 
00045     dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2;
00046     dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl)) / entraxe;
00047 
00048     xB = xA + dDist * cos(phiA);
00049     yB = yA + dDist * sin(phiA);
00050     phiB = phiA + dAngl;
00051 
00052     comptG = 0;
00053     comptD = 0;
00054 }
00055 //*/
00056 
00057 /*
00058 ///// 2) ODOMÉTRIE Geonobot : Approximation par arc de cercle
00059 
00060 #define entraxe 245 // Distance entre les roues codeuses
00061 double xB = 0, yB = 0, phiB = 0; // Nouvelles coordonnées
00062 double xA = 0, yA = 0, phiA = 0; // Dernières coordonnées calculées
00063 double xO = 0, yO = 0; // Centre du cercle de rayon R
00064 double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
00065 double distG = 0, distD = 0; // Distance parcourue par chaque roue
00066 double rayon = 0;
00067 
00068 void odoGeonobot2()
00069 {
00070     xA = xB;
00071     yA = yB;
00072     phiA = phiB;
00073 
00074     dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2;
00075     dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl)) / entraxe;
00076     rayon = dDist / dAngl;
00077 
00078     xO = xA - R*sin(phiA);
00079     yO = yA + R*cos(phiA);
00080 
00081     phiB = phiA + dAngl;
00082 
00083     if (dAngl == 0) {
00084         xB = xO + R*sin(phiB);
00085         yB = yO + R*cos(phiB);
00086     }
00087 
00088     else {
00089         xB = xO + dDist*cos(phiB);
00090         yB = yO + dDist*sin(phiB);
00091     }
00092 
00093     comptG = 0;
00094     comptD = 0;
00095 }
00096 */