Réception des coordonnées des 3 blobs des goals (PAS ENCORE DE FILTRES)

LudovaTech · Jun 16, 2024 · 2591492 · 2591492
1 parent fa6670a
commit 2591492
Show file tree

Hide file tree

Showing 5 changed files with 35 additions and 27 deletions.
diff --git a/src/lidar_analyzer_anc.cpp b/src/lidar_analyzer_anc.cpp
@@ -246,7 +246,7 @@ LidarInfosGlue getLidarInfos(FieldProperties fP, bool readFromLidar = true, bool
     // unsigned long elapsed = millis() - start_millis;
     // SerialDebug.println("Temps de récupération des données du lidar : " + String(elapsed) + "ms");
     // start_millis = millis();
-  } else {  // pour tester sans lidar
+  } else {  // pour tester sans lidar    
     if (input == nullptr) {
       input = "(2423,178);(2494,178);(2565,179);(2636,181);(2707,184);(2778,186);(2849,189);(2920,192);(2991,195);(3062,199);(3133,203);(3204,210);(13634,481);(13710,488);(13786,489);(13862,493);(13938,504);(14014,168);(14090,158);(14166,157);(14242,157);(14318,552);(14394,561);(14470,573);(14540,585);(14612,595);(14684,606);(14756,618);(14828,630);(14900,643);(14972,656);(15044,669);(15116,684);(15188,700);(15260,717);(15332,735);(15400,754);(15471,773);(15542,389);(15613,375);(15684,370);(15755,369);(15826,368);(15897,366);(15968,359);(16039,357);(16110,354);(16181,351);(16262,349);(16333,348);(16404,348);(16475,349);(16546,351);(16617,353);(16688,357);(16759,365);(16830,371);(16901,379);(16972,1237);(17043,1237);(17115,1237);(17187,1237);(18826,1250);(18898,1251);(18970,295);(19042,294);(19114,288);(19186,1262);(19258,1263);(19330,1267);(19402,1270);(19474,1274);(19546,1277);(19618,1281);(19691,1290);(19762,1300);(19833,1307);(19904,1312);(19975,1313);(20046,1316);(20117,1317);(20188,1317);(20259,1319);(20330,1322);(20401,1329);(20472,1335);(20547,1341);(20618,1347);(20689,1354);(20760,1361);(20831,1368);(20902,1375);(20973,1383);(21044,1391);(21115,1402);(21186,1413);(21257,1423);(21328,1432);(21404,1440);(21474,1446);(21544,1452);(21614,1460);(21684,1469);(21754,1486);(21824,1499);(21894,1507);(21964,288);(22034,1534);(22104,1565);(22174,1585);(22255,1603);(22328,1620);(22401,1639);(22474,1649);(22547,1657);(22620,1666);(22693,1679);(22766,1805);(22839,1825);(22912,1827);(22985,1827);(23058,1825);(23133,1834);(23204,1860);(23275,1915);(23346,1929);(23417,1913);(23772,1740);(23843,1729);(23914,1714);(23994,1698);(24057,1682);(24120,1671);(24183,1660);(24246,1648);(24309,1636);(24372,1625);(24435,1615);(24498,1605);(24561,459);(24624,438);(24687,426);(24758,418);(24830,409);(24902,400);(24974,392);(25046,384);(25118,376);(25190,367);(25262,360);(25334,353);(25406,346);(25478,340);(25550,334);(25623,328);(25695,322);(25767,320);(25839,316);(25911,314);(25983,313);(26055,314);(26127,316);(26199,318);(26271,320);(26343,323);(26415,327);(26490,330);(26560,333);(26630,336);(26700,339);(26770,342);(26840,345);(26910,348);(26980,352);(27050,359);(27120,364);(27190,369);(27260,374);(27332,380);(27403,386);(27474,392);(27545,399);(27616,406);(27687,414);(27758,424);(27829,430);(27900,1477);(27971,1483);(28042,1489);(28113,1495);(28190,1500);(28263,1506);(28336,1512);(28409,1519);(28482,1525);(28555,1530);(28628,1536);(28701,1542);(28774,1549);(28847,1556);(28920,1563);(28993,1571);(29061,1579);(29132,1587);(29203,1594);(29274,1602);(29345,1611);(29416,1618);(29487,1626);(29558,1634);(29629,1642);(29700,1649);(29771,1657);(29842,1667);(29923,1681);(29993,1694);(30063,1705);(30133,1717);(30203,1729);(30273,1742);(30343,1755);(30413,1770);(30483,1785);(30553,1798);(30623,1813);(30693,1827);(30761,1841);(30832,1855);(30903,1868);(30974,1884);(31045,1886);(31116,1887);(31187,1856);(31258,1831);(31329,1811);(31400,1791);(31471,1769);(31542,1747);(31623,1725);(31701,1704);(31779,1684);(31857,1664);(31935,1647);(32013,1632);(32091,1617);(32169,1602);(32247,1587);(32325,1570);(32403,1555);(32481,1541);(32558,1526);(32629,1511);(32700,1499);(32771,1487);(32842,1475);(32913,1463);(32984,1454);(33055,1445);(33126,1436);(33197,1426);(33268,1417);(33339,1409);(33420,1401);(33491,1392);(33562,1384);(33633,1376);(33704,1368);(33775,1360);(33846,1353);(33917,1346);(33988,1339);(34059,1332);(34130,1325);(34201,677);(34274,665);(34344,663);(34414,662);(34484,662);(34554,663);(34624,666);(34694,673);(34764,680);(34834,687);(34904,695);(34974,704);(35044,719);(35118,744);(35189,774);(35260,793);(35331,579);(712,162);(789,201);(860,200);(931,198);(1002,195);(1073,191);(1144,188);(1215,185);(1286,183);(1357,181);(1428,179);(1499,178);(1570,177);(1650,177);(1724,177);(1798,176);(1872,176);(1946,176);(2020,176);(2094,176);(2168,176);(2242,176);(2316,176);(2390,177);(2464,178);(2543,179);(2615,180);(2687,182);(2759,185);(2831,188);(2903,191);(2975,194);(3047,197);(3119,200);(3191,204);(3263,207);(3335,216);(3414,229);(3485,590);(3556,584);(3627,577);(3698,569);(3769,559);(3840,550);(3911,542);(3982,534);(4053,527);(4124,519);(4195,511);(4272,504);(4343,497);(4414,490);(4485,484);(4556,478);(4627,473);(4698,467);(4769,462);(4840,457);(4911,452);(4982,447);(5053,442);(5132,440);(5203,436);(5274,432);(5345,428);(5416,424);(5487,420);(5558,416);(5629,412);(5700,409);(5771,405);(5842,402);(5913,399);(5990,396);(6055,393);(6120,391);(6185,388);(6250,386);(6315,383);(6380,381);(6445,379);(6510,376);(6575,374);(6640,372);(6705,370);(6779,368);(6850,367);(6921,365);(6992,363);(7063,361);(7134,359);(7205,357);(7276,356);(7347,354);(7418,353);(7489,352);(7560,351);(7636,350);(7712,349);(7788,348);(7864,347);(7940,347);(8016,346);(8092,345);(8168,345);(8244,344);(8320,343);(8396,343);(8472,343);(8547,343);(8619,343);(8691,342);(8763,342);(8835,342);(8907,342);(8979,342);(9051,342);(9123,342);(9195,342);(9267,342);(9339,343);(9414,343);(9485,343);(9556,343);(9627,343);(9698,344);(9769,345);(9840,346);(9911,347);(9982,347);(10053,348);(10124,349);(10195,350);(10272,351);(10348,352);(10424,353);(10500,353);(10576,352);(10652,350);(10728,350);(10804,356);(11253,365);(11395,380);(11466,381);(11537,377);(11608,372);(11679,373);(11750,380);(11821,386);(11892,391);(11963,396);(12047,397);(12118,401);(12189,404);(12260,407);(12331,410);(12402,413);(12473,416);(12544,418);(12615,420);(12686,423);(12757,425);(12828,428);(12904,432);(12975,436);(13046,443);(13117,445);(13188,452);(13259,457);(13330,462);(13401,464);(13472,471);(13543,476);(13614,481);(13685,486);(13761,488);(13831,492);(13901,499);(13971,508);(14041,157);(14111,156);(14181,155);(14251,161);(14321,556);(14391,565);(14461,577);(14531,588);(14611,599);(14684,611);(14757,624);(14830,636);(14903,648);(14976,662);(15049,675);(15122,691);(15195,708);(15268,726);(15341,744);(15414,761);(15493,776);(15564,380);(15635,372);(15706,371);(15777,370);(15848,368);(15919,362);(15990,357);(16061,355);(16132,352);(16203,350);(16274,348);(16350,348);(16420,348);(16490,350);(16560,352);(16630,355);(16700,362);(16770,368);(16840,376);(16910,385);(16980,1230);(17050,1232);(17120,1234);(17200,1233);(18815,1249);(18890,1250);(18968,279);(19040,289);(19112,301);(19184,1257);(19256,1259);(19328,1263);(19400,1267);(19472,1271);(19544,1278);(19616,1280);(19688,1287);(19760,1296);(19832,1305);(19903,1311);(19974,1313);(20045,1315);(20116,1316);(20187,1316);(20258,1318);(20329,1321);(20400,1328);(20471,1333);(20542,1338);(20613,1344);(20690,1350);(20756,1357);(20822,1364);(20888,1372);(20954,1380);(21020,1388);(21086,1397);(21152,1406);(21218,1416);(21284,1425);(21350,1434);(21416,1442);(21489,1449);(21560,1457);(21631,1468);(21702,1481);(21773,1494);(21844,1505);(21915,293);(21986,293);(22057,1550);(22128,1577);(22199,1596);(22270,1615);(22340,1632);(22416,1642);(22492,1648);(22568,1660);(22644,1672);(22720,1674);(22796,1819);(22872,1825);(22948,1826);(23024,1826);(23100,1813);(23176,1852)";
     }

diff --git a/src/lidar_reader.cpp b/src/lidar_reader.cpp
@@ -201,6 +201,7 @@ std::vector<LidarPoint> CircularLidarPointsBuffer::getPoints() {
       // Now the stream is aligned
       byte buffer[45];
       size_t nbrBytesReceived = SerialLidar.readBytes(buffer, 45);
+      //SerialDebug.println(String((char*)buffer));
       if (nbrBytesReceived != 45) {
         // SerialDebug.println("error, wrong number of bytes received (" + String(nbrBytesReceived) + ")");
       } else {

diff --git a/src/movements.cpp b/src/movements.cpp
@@ -113,7 +113,6 @@ void Motors::goTo(Vector2 vector, int celerity, Radians orientation) const {
     float speedBR = MBRcelerity * rapport;
     float speedBL = MBLcelerity * rapport;
 
-    SerialDebug.println("Orientation dans movements : " + String(orientation));
     float minimumSpeed = min(speedFR, min(speedFL, min(speedBR, speedBL)));
     Radians rotation = orientation * celerity * 0.6;
 

diff --git a/src/src.ino b/src/src.ino
@@ -31,11 +31,18 @@ struct CamInfosGlue {
   Optional<EnemyGoalPos> enemyGoalPos;
 };
 
+uint8_t bigserialbuffer[4000];
+//uint8_t bigserialbufferlidar[4000];
+
+
 void setup() {
   SerialDebug.begin(115200);
   SerialCam.begin(115200);
   SerialLidar.begin(230400);
 
+  SerialCam.addMemoryForRead(&bigserialbuffer, sizeof(bigserialbuffer));
+  //SerialLidar.addMemoryForRead(&bigserialbufferlidar, sizeof(bigserialbufferlidar));
+
   SerialCam.setTimeout(10);
   SerialLidar.setTimeout(10);
 
@@ -60,46 +67,49 @@ CamInfosGlue getCamInfos() {
   // SerialDebug.println("nb of bytes available: " + String(bytesAvailable));
 
   if (bytesAvailable >= 26) {
-    byte buffer[65];
-    size_t nbrBytesReceived = SerialCam.readBytes(buffer, min(bytesAvailable, sizeof(buffer) - 1));
-    buffer[nbrBytesReceived] = '\0';
-    // SerialDebug.println(" reçu: " + String((char*)buffer));
+    //byte buffer[301];
+    size_t nbrBytesReceived = SerialCam.readBytes(bigserialbuffer, min(bytesAvailable, sizeof(bigserialbuffer) - 1));
+    bigserialbuffer[nbrBytesReceived] = '\0';
+    //SerialDebug.println(" reçu: " + String((char*)bigserialbuffer));
 
-    std::string lastCompleteSequence = extractLastCompleteSequence((char*)buffer);
+    std::string lastCompleteSequence = extractLastCompleteSequence((char*)bigserialbuffer);
     if (!lastCompleteSequence.empty()) {
-      // exemple : b-096-121+000+000+000+000e
-      int ball_x, ball_y, my_goal_x, my_goal_y, enemy_goal_x, enemy_goal_y;
-      if (sscanf(lastCompleteSequence.c_str(), "b%d%d%d%d%d%de", &ball_x, &ball_y, &my_goal_x, &my_goal_y,
-                 &enemy_goal_x, &enemy_goal_y) == 6) {
-        SerialDebug.println("Position balle: x=" + String(ball_x) + ", y=" + String(ball_y) + ", my goal x=" +
-                            String(my_goal_x) + ", y=" + String(my_goal_y) + ", ennemy goal x=" + String(enemy_goal_x) + ", y=" + String(enemy_goal_y));
+      // exemple : b+048+019+006+065-045+027+000+000+090+015+065+070+066-059e
+
+      int ballX, ballY, 
+          myGoalX1, myGoalY1, myGoalX2, myGoalY2, myGoalX3, myGoalY3, 
+          enemyGoalX1, enemyGoalY1, enemyGoalX2, enemyGoalY2, enemyGoalX3, enemyGoalY3;
+
+      if (sscanf(lastCompleteSequence.c_str(), "b%d%d%d%d%d%d%d%d%d%d%d%d%d%de", 
+                &ballX, &ballY, &myGoalX1, &myGoalY1, &myGoalX2, &myGoalY2, &myGoalX3, &myGoalY3, 
+                &enemyGoalX1, &enemyGoalY1, &enemyGoalX2, &enemyGoalY2, &enemyGoalX3, &enemyGoalY3) == 14) {
+
+        SerialDebug.println("Position balle: x=" + String(ballX) + ", y=" + String(ballY) + ", my goal x=" +
+                            String(myGoalX1) + ", y=" + String(myGoalY1) + ", ennemy goal x=" + String(enemyGoalX1) + ", y=" + String(enemyGoalY1));
         Optional<BallPos> bP;
-        if (ball_x != 0 && ball_y != 0) {
-          bP = BallPos(ball_x, ball_y);
+        if (ballX != 0 && ballY != 0) {
+          bP = BallPos(ballX, ballY);
         }
         Optional<MyGoalPos> mGP;
-        if (my_goal_x != 0 && my_goal_y != 0) {
-          mGP = MyGoalPos(my_goal_x, my_goal_y);
+        if (myGoalX1 != 0 && myGoalY1 != 0) {
+          mGP = MyGoalPos(myGoalX1, myGoalY1);
         }
         Optional<EnemyGoalPos> eGP;
-        if (enemy_goal_x != 0 && enemy_goal_y != 0) {
-          eGP = EnemyGoalPos(enemy_goal_x, enemy_goal_y);
+        if (enemyGoalX1 != 0 && enemyGoalY1 != 0) {
+          eGP = EnemyGoalPos(enemyGoalX1, enemyGoalY1);
         }
         CamInfosGlue cIG{
             bP,
             mGP,
             eGP};
 
-        SerialDebug.println("ballPos has value : " + String(cIG.ballPos.hasValue()));
-        SerialDebug.println("myGoalPos has value : " + String(cIG.myGoalPos.hasValue()));
-        SerialDebug.println("enemyGoalPos has value : " + String(cIG.enemyGoalPos.hasValue()));
         return cIG;
       } else {
         SerialDebug.println("Erreur lors de l'extraction des données de la caméra: " + String(lastCompleteSequence.c_str()));
       }
 
     } else {
-      SerialDebug.println("Aucune séquence complète trouvée, reçu: " + String((char*)buffer));
+      SerialDebug.println("Aucune séquence complète trouvée, reçu: " + String((char*)bigserialbuffer));
     }
   }
   return CamInfosGlue();
@@ -166,7 +176,6 @@ void loop() {
       }
     }
   }*/
-
   // DOING ACTION
   FutureAction currentAction = chooseStrategy(
       fieldProperties,

diff --git a/src/strategy.cpp b/src/strategy.cpp
@@ -50,8 +50,8 @@ FutureAction chooseStrategy(
     if (leavingField_B(fP, oLBI.value())) {
       return refrainLeavingField_B(fP, oLBI.value());
     }
-  } else {
-    return FutureAction::stopRobot();
+  // } else {
+  //   return FutureAction::stopRobot();
   }
   if (oMGP.hasValue()) {
     if (enterInMyGoal_C(fP, oMGP.value())) {
@@ -66,7 +66,6 @@ FutureAction chooseStrategy(
 
   // Then we choose the appropriate Strategy
   if (!oBP.hasValue()) {
-    SerialDebug.println(String(oBP.hasValue()));
     // We don't know where is the ball
     if (oLDI.hasValue()) {
       return slalowingBackwards_D(fP, oLDI.value());