Revision 6:ea6b30c4bb01, committed 2020-07-26

Comitter:

Nanaud

Date:

Sun Jul 26 09:24:31 2020 +0000

Parent:

5:34ed652f8c31

Child:

7:8982d9375363

Commit message:

Fusion des fichiers codeurs et odo_asserv

Changed in this revision

--- a/codeurs.cpp	Tue Jul 21 19:33:38 2020 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,15 +0,0 @@
-// Nom du fichier : codeurs.cpp
-#include "pins.h"
-
-// Variables globales
-long cpt_cdgA=0; // Codeur de gauche
-long cpt_cddA=0; // Codeur de droite
-
-void cdgaRise(){
-    if(cdgB) cpt_cdgA++;
-    else cpt_cdgA--;
-}
-
-void cddaRise(){
-    
-}

--- a/codeurs.h	Tue Jul 21 19:33:38 2020 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,11 +0,0 @@
-// Nom du fichier : codeurs.h
-
-// extern
-extern long cpt_cdgA;
-//extern long cpt_cdgB;
-extern long cpt_cddA;
-//extern long cpt_cddB;
-
-// Prototypes
-void cdgaRise();
-//void cdgbRise();
\ No newline at end of file

--- a/debugPC.cpp	Tue Jul 21 19:33:38 2020 +0000
+++ b/debugPC.cpp	Sun Jul 26 09:24:31 2020 +0000
@@ -290,9 +290,9 @@
 
 void affCodeurs()
 {
-    if(aff_cd[0]) pc.printf("CdgA = %d\n\r", cpt_cdgA);
-    //if(aff_cd[1]) pc.printf("CdgB = %d\n\r", cpt_cdgB);
+    if(aff_cd[0]) pc.printf("comptG = %d\n\r", comptG);
+    if(aff_cd[1]) pc.printf("comptD = %d\n\r", comptD);
 
-    if(aff_cd[0]) bt.printf("CdgA = %d\n\r", cpt_cdgA);
-    //if(aff_cd[1]) bt.printf("CdgB = %d\n\r", cpt_cdgB);
+    if(aff_cd[0]) bt.printf("comptG = %d\n\r", comptG);
+    if(aff_cd[1]) bt.printf("comptD = %d\n\r", comptD);
 }

--- a/main.cpp	Tue Jul 21 19:33:38 2020 +0000
+++ b/main.cpp	Sun Jul 26 09:24:31 2020 +0000
@@ -1,14 +1,35 @@
 // Nom du fichier : main.cpp
 #include "pins.h"
 
+void btnFct(){
+    mot_dis();
+    aff_cd[0] = 0;
+    aff_cd[1] = 0;
+    
+    pc.printf("comptG = %d\r\n",comptG);
+    pc.printf("comptD = %d\r\n",comptD);
+    bt.printf("comptG = %d\r\n",comptG);
+    bt.printf("comptD = %d\r\n",comptD);
+}
+
 int main()
 {
     pc.printf("\r\nAresCDFMainCode\r\n");
     bt.printf("\r\nAresCDFMainCode\r\n");
 
+    btn.rise(&btnFct);
+
     // debug
     pc.attach(&serialIT); // Interruption liaison série
     bt.attach(&bluetoothIT); // Interruption  bluetooth
+    pc.baud(9600);
+    pc.format(8,SerialBase::None,1);
+    bt.baud(9600);
+    bt.format(8,SerialBase::None,1);
+
+    ticker_US.attach(&captUS_trig,0.2); // On apelle cette fonction toutes 0.2 secondes
+    ticker_affUS.attach(&affUltrasons,1.0);
+    ticker_affcd.attach(&affCodeurs,1.0);
 
     // Init capteurs à ultrasons
     captUS_init();
@@ -30,22 +51,13 @@
 
     // Init codeurs
     cdgA.rise(&cdgaRise);
-    //cdgB.rise(&cdgbRise);
+    cddA.rise(&cddaRise);
     cdgA.mode(PullUp);
-    cdgB.mode(PullUp);
+    cddA.mode(PullUp);
+    
+    // Odometrie
 
-    // Fonctions d'affichage pour les tests
-    ticker_US.attach(&captUS_trig,0.2); // On apelle cette fonction toutes 0.2 secondes
-    ticker_affUS.attach(&affUltrasons,1.0);
-    ticker_affcd.attach(&affCodeurs,1.0);
-    //aff_cd[0]=!aff_cd[0];
 
-    //test_drv();
-
-    pc.baud(9600);
-    pc.format(8,SerialBase::None,1);
-    bt.baud(9600);
-    bt.format(8,SerialBase::None,1);
 
     while(1) {}
 }

--- a/motors.cpp	Tue Jul 21 19:33:38 2020 +0000
+++ b/motors.cpp	Sun Jul 26 09:24:31 2020 +0000
@@ -5,9 +5,10 @@
 {
     mot_dis();
     motGauche_fwd();
-    motDroite_fwd();
-    drvGauche.moveLinSpeed(0.1);
-    drvDroite.moveLinSpeed(0.1);
+    //motDroite_fwd();
+    motDroite_bck();
+    drvGauche.moveLinSpeed(0.05);
+    drvDroite.moveLinSpeed(0.05);
     mode = 0b111; // M0, M1 et M2 sont à 1
 }
 
@@ -46,8 +47,9 @@
     drvDroite.setDir(FORWARD);
 }
 
-// TESTS
 
+// FONCTIONS TESTS
+//
 void test_drv()
 {
     mot_en();

--- a/odo_asserv.cpp	Tue Jul 21 19:33:38 2020 +0000
+++ b/odo_asserv.cpp	Sun Jul 26 09:24:31 2020 +0000
@@ -1,25 +1,111 @@
 //Nom du fichier : odo_asserv.cpp
 #include "pins.h"
 
-// Variables globales
-// cpt_cdgA est le compteur d'impulsion du codeur de gauche
-// cpt_cddA est le compteur d'impulsion du codeur de droite
-float posX, posY; // Position et orientation 
-float theta; 
+///// 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 odometrie(){
+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;
 
-    float distG = (cpt_cdgA * perimetreRoueCodeuse) / NbPulseCodeur; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
-    float distD = (cpt_cddA * perimetreRoueCodeuse) / NbPulseCodeur; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
+    // 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)));
     
-    float distRobot = (distG + distD) / 2; // Distance parcourue par le robot (en mm)
-    float rayon = (ecartCodeuse /2) * ((distD + distG) / (distD - distD)); // Trajectoire s'apparente à un cercle
+    // On détermine les commandes à envoyer aux moteurs
+    cmdD = abs((float)distanceCible);
+    if(cmdD>255)
+    {
+        cmdD = 255;
+    }
+    cmdG = cmdD;
     
-    float dTheta = distRobot / rayon; // Changement d'orientation => Commande
-    float posX0 = posX - rayon*cos(theta); 
-    float posY0 = posY - rayon*sin(theta);
-    
-    theta = theta + dTheta; // Mise à jour des coordonnées
-    posX = posX0 + rayon*cos(theta);
-    posY = posY0 + rayon*sin(theta);
-}
\ No newline at end of file
+    // 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;
+    }
+}
+

--- a/odo_asserv.h	Tue Jul 21 19:33:38 2020 +0000
+++ b/odo_asserv.h	Sun Jul 26 09:24:31 2020 +0000
@@ -1,3 +1,13 @@
-// Nom du fichier : odo.h
+// Nom du fichier : odo_asserv.h
+
+// extern
+// CODEURS
+//extern long cpt_cdgA;
+//extern long cpt_cddA;
+extern long comptG;
+extern long comptD;
 
 
+// Prototypes
+void cdgaRise();
+void cddaRise();

--- a/pins.cpp	Tue Jul 21 19:33:38 2020 +0000
+++ b/pins.cpp	Sun Jul 26 09:24:31 2020 +0000
@@ -1,6 +1,8 @@
 // Nom du fichier : pins.cpp
 #include "pins.h"
 
+InterruptIn btn(PC_13);
+
 // Capteurs à ultrasons
 DigitalOut trigger(PB_9);
 InterruptIn echo1(PA_11);

--- a/pins.h	Tue Jul 21 19:33:38 2020 +0000
+++ b/pins.h	Sun Jul 26 09:24:31 2020 +0000
@@ -7,7 +7,6 @@
 #include "debug.h"
 #include "captUS.h"
 #include "motors.h"
-#include "codeurs.h"
 #include "odo_asserv.h"
 
 // #define
@@ -19,6 +18,8 @@
 #define vtSAT 0.250 // m/s 
 #define perimetreE Pi*ecartCodeuse
 
+extern InterruptIn btn;
+
 //Capteurs à ultrasons
 extern DigitalOut trigger;
 extern InterruptIn echo1;