AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed DRV8825

Files at this revision

API Documentation at this revision

Revision 24:be2b2be6907b, committed 2020-10-28

Comitter:
Nanaud
Date:
Wed Oct 28 20:08:39 2020 +0000
Parent:
23:a74135a0271d
Child:
25:869b1c1f51a7
Commit message:
Homologue

Changed in this revision

captUS.cpp Show annotated file Show diff for this revision Revisions of this file
captUS.h Show annotated file Show diff for this revision Revisions of this file
debugPC.cpp Show annotated file Show diff for this revision Revisions of this file
odo_asserv.cpp Show annotated file Show diff for this revision Revisions of this file
odo_asserv.h Show annotated file Show diff for this revision Revisions of this file 
--- a/captUS.cpp	Wed Oct 28 16:27:05 2020 +0000
+++ b/captUS.cpp	Wed Oct 28 20:08:39 2020 +0000
@@ -1,22 +1,55 @@
 /* #include */
 #include "pins.h"
 
+/* #define */
+#define DISTLIM 600
+#define MAXMOY 3
+
 /* Variables globales */
 Timer TimUS;
 Ticker TickUS_actu;
 
-const float DISTLIM = 450;
+//const float DISTLIM = 600;
 
-double us_high[6]= {0};
-double us_low[6]= {0};
-double us_diff[6]= {0};
+unsigned int us_high[6]= {0};
+unsigned int us_low[6]= {0};
+unsigned int us_diff[6]= {0};
 bool us_verif[6]= {0};
 
-double distt[6];
+unsigned int distt[6];
 bool rebooted = 0;
 bool wtt = 0;
 
-//int sptt = 0;
+int indix = 0;
+unsigned int total[6] = {0};
+unsigned int ttt[6] = {0};
+
+void calculTotal()
+{
+    if(indix < MAXMOY) {
+        for(int j=0; j<6 ; j++) {
+            total[j]+=distt[j];
+        }
+        indix++;
+    } else {
+        for(int j=0; j<6 ; j++) {
+            ttt[j]=total[j]/MAXMOY;
+            total[j]=0;
+        }
+
+        /*
+        pc.printf("ttt[1] = %d\n\r", ttt[0]);
+        pc.printf("ttt[2] = %d\n\r", ttt[1]);
+        pc.printf("ttt[3] = %d\n\r", ttt[2]);
+        pc.printf("ttt[4] = %d\n\r", ttt[3]);
+        pc.printf("ttt[5] = %d\n\r", ttt[4]);
+        pc.printf("ttt[6] = %d\n\r", ttt[5]);
+        pc.printf("\n\r");
+        */
+
+        indix=0;
+    }
+}
 
 /*
 void captUS_init()
@@ -31,9 +64,12 @@
 {
     convertToDistance();
 
+    calculTotal();
+
     if((objRecule[indice]==0) && (fnc == 2)) {
         //if ((distt[5] >= DISTLIM) && (distt[0] >= DISTLIM)  && (distt[1] >= DISTLIM)) {
-        if (((distt[5] >= DISTLIM) && (distt[0] >= DISTLIM)  && (distt[1] >= DISTLIM)) || ((distt[5] < 0) && (distt[0] < 0)  && (distt[1] < 0))) {
+        //if (((distt[5] >= DISTLIM) && (distt[0] >= DISTLIM)  && (distt[1] >= DISTLIM)) /*|| ((distt[5] < 0) && (distt[0] < 0)  && (distt[1] < 0))*/) {
+        if ((ttt[5] >= DISTLIM) && (ttt[0] >= DISTLIM)  && (ttt[1] >= DISTLIM)) {
             wtt = 1;
         }
 
@@ -46,7 +82,8 @@
 
     else if((objRecule[indice]==1) && (fnc == 2)) {
         //if ((distt[2] >= DISTLIM) && (distt[3] >= DISTLIM) && (distt[4] >= DISTLIM)) {
-        if (((distt[2] >= DISTLIM) && (distt[3] >= DISTLIM)  && (distt[4] >= DISTLIM)) || ((distt[2] < 0) && (distt[3] < 0)  && (distt[4] < 0))) {
+        //if (((distt[2] >= DISTLIM) && (distt[3] >= DISTLIM)  && (distt[4] >= DISTLIM)) /*|| ((distt[2] < 0) && (distt[3] < 0)  && (distt[4] < 0))*/) {
+        if ((ttt[2] >= DISTLIM) && (ttt[3] >= DISTLIM)  && (ttt[4] >= DISTLIM)) {
             wtt=1;
         }
 
@@ -76,14 +113,18 @@
     pc.printf("Dist6 = %5.0lf mm\n\r", ::distance[5]);
     pc.printf("\n\r");
     */
+    
+    /*
+    pc.printf("ttt[1] = %d\n\r", ttt[0]);
+    pc.printf("ttt[2] = %d\n\r", ttt[1]);
+    pc.printf("ttt[3] = %d\n\r", ttt[2]);
+    pc.printf("ttt[4] = %d\n\r", ttt[3]);
+    pc.printf("ttt[5] = %d\n\r", ttt[4]);
+    pc.printf("ttt[6] = %d\n\r", ttt[5]);
+    pc.printf("\n\r");
+    */
 
-    pc.printf("distance[1] = %lf\n\r", distt[0]);
-    pc.printf("distance[2] = %lf\n\r", distt[1]);
-    pc.printf("distance[3] = %lf\n\r", distt[2]);
-    pc.printf("distance[4] = %lf\n\r", distt[3]);
-    pc.printf("distance[5] = %lf\n\r", distt[4]);
-    pc.printf("distance[6] = %lf\n\r", distt[5]);
-    pc.printf("\n\r");
+    //calculTotal();
 
     TimUS.reset();
     trigger=1;
@@ -203,7 +244,7 @@
      **************************************/
 
     for(int i = 0; i<6; i++) {
-        distt[i] = ((us_diff[i]*340)/1000); //conversion en distance(mm)
+        distt[i] = (int) ((us_diff[i]*340)/1000); //conversion en distance(mm)
     }
 
     /****************************************

--- a/captUS.h	Wed Oct 28 16:27:05 2020 +0000
+++ b/captUS.h	Wed Oct 28 20:08:39 2020 +0000
@@ -13,14 +13,14 @@
 // extern
 //extern double us_out[6];
 
-extern double us_high[6];
-extern double us_low[6];
-extern double us_diff[6];
+extern unsigned int us_high[6];
+extern unsigned int us_low[6];
+extern unsigned int us_diff[6];
 extern bool us_verif[6];
 
 extern Timer TimUS;
 extern Ticker TickUS_actu;
-extern double distt[6];
+extern unsigned int distt[6];
 
 extern bool rebooted;
 extern bool wtt;

--- a/debugPC.cpp	Wed Oct 28 16:27:05 2020 +0000
+++ b/debugPC.cpp	Wed Oct 28 20:08:39 2020 +0000
@@ -292,13 +292,13 @@
             pc.printf("consigneAngl = %lf\n\r", consigneOrientation);
             pc.printf("consigneDist = %lf\n\r", distanceCible);
             pc.printf("indice = %d\n\r", indice);
-            pc.printf("dist 1 = %lf\n\r", distt[0]);
-            pc.printf("dist 2 = %lf\n\r", distt[1]);
-            pc.printf("dist 6 = %lf\n\r", distt[5]);
+            //pc.printf("dist 1 = %d\n\r", ttt[0]);
+            //pc.printf("dist 2 = %d\n\r", ttt[1]);
+            //pc.printf("dist 6 = %d\n\r", ttt[5]);
             pc.printf("---\n\r");
-            pc.printf("dist 3 = %lf\n\r", distt[2]);
-            pc.printf("dist 4 = %lf\n\r", distt[3]);
-            pc.printf("dist 5 = %lf\n\r", distt[4]);
+            //pc.printf("dist 3 = %d\n\r", ttt[2]);
+            //pc.printf("dist 4 = %d\n\r", ttt[3]);
+            //pc.printf("dist 5 = %d\n\r", ttt[4]);
 
             bt.printf("Odometrie :\n\r");
             bt.printf("x = %lf\n\r", x);

--- a/odo_asserv.cpp	Wed Oct 28 16:27:05 2020 +0000
+++ b/odo_asserv.cpp	Wed Oct 28 20:08:39 2020 +0000
@@ -2,8 +2,8 @@
 #include "pins.h"
 
 /* #define & constantes */
-#define VMAXROT 0.050
-#define VMAXLIN 0.080
+#define VMAXROT 0.020
+#define VMAXLIN 0.040
 #define entraxe 253 // (Valeur théorique = 255)
 const double coeffGLong = 5.956, coeffDLong = 5.956; // tics/millimètre
 
@@ -16,10 +16,20 @@
 int objRecule[NbObj]= {0,0,0,0,1,0,0,1,0,0,1,0,0};
 */
 
+/*
 int objEtape[NbObj] = {0,1,1,1,1,1,1,1,1,1,1,1,1}; // Stratégie côté bleu
 double objX[NbObj] =  {110,645,645,200,645,336,189,200,645,468,645,371,215};
 double objY[NbObj] =  {1085,1200,1400,1400,1400,1788,1032,1200,920,920,920,519,780};
 int objRecule[NbObj]= {0,0,0,0,1,0,0,1,0,0,1,0,0};
+*/
+
+
+//int objEtape[NbObj] = {0,1,1,1}; // Stratégie côté bleu
+int objEtape [4] = {0,1,1,1};
+double objX[4] = {0,660, 660, 210};
+double objY[4] = {0,1070,1650,1300};
+int objRecule[NbObj]= {0,0,0,0};
+
 
 /* Variable globale */
 Ticker Ticker_asserv;
@@ -72,14 +82,14 @@
 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
+const double coeffPro = 0.020; // 0.023 de base
+const double coeffDer = 0.010; // 0.023 de base
 
 // Ligne droite
 double erreurPreDist = 0;
 double deltaErreurDist = 0;
-const double coeffProDist = 0.0004; // 0.010 de base
-const double coeffDerDist = 0.0004; // 0.010 de base
+const double coeffProDist = 0.0008; // 0.010 de base
+const double coeffDerDist = 0.0008; // 0.010 de base
 
 // NEW NEW NEW NEW
 int fnc = 0;
@@ -163,7 +173,7 @@
             double deltaCommande = (abs(coeffPro * erreurAngle) + abs(coeffDer * deltaErreur));
 
             if(acc) {
-                cmdG = cmdG + 0.0001; // +0.0008
+                cmdG = cmdG + 0.0008; // +0.0008
                 cmdD = cmdG;
 
                 if (cmdG >= VMAXROT) acc = 0;
@@ -211,7 +221,7 @@
             double deltaCommande2 = (abs(coeffProDist * distanceCible) + abs(coeffDerDist * deltaErreurDist));
 
             if(acc) {
-                cmdG = cmdG + 0.0001; // +0.0008
+                cmdG = cmdG + 0.0006; // +0.0008
                 cmdD = cmdG;
 
                 if (cmdG >= VMAXLIN) {

--- a/odo_asserv.h	Wed Oct 28 16:27:05 2020 +0000
+++ b/odo_asserv.h	Wed Oct 28 20:08:39 2020 +0000
@@ -1,6 +1,6 @@
 // Nom du fichier : odo_asserv.h
 
-#define NbObj 13
+#define NbObj 4
 
 extern int indice;
 extern int objEtape[NbObj];