From 2591492c81a769e8fc32543d22e12c30f15b7fd3 Mon Sep 17 00:00:00 2001 From: RemyMagnon Date: Sun, 16 Jun 2024 21:25:31 +0200 Subject: [PATCH] =?UTF-8?q?R=C3=A9ception=20des=20coordonn=C3=A9es=20des?= =?UTF-8?q?=203=20blobs=20des=20goals=20(PAS=20ENCORE=20DE=20FILTRES)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/lidar_analyzer_anc.cpp | 2 +- src/lidar_reader.cpp | 1 + src/movements.cpp | 1 - src/src.ino | 53 ++++++++++++++++++++++---------------- src/strategy.cpp | 5 ++-- 5 files changed, 35 insertions(+), 27 deletions(-) diff --git a/src/lidar_analyzer_anc.cpp b/src/lidar_analyzer_anc.cpp index 88b6461..d7e9f1f 100644 --- 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 index 51a6341..ec9954e 100644 --- a/src/lidar_reader.cpp +++ b/src/lidar_reader.cpp @@ -201,6 +201,7 @@ std::vector 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 index 941f9a7..b9e85ef 100644 --- 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 index 45f8f1d..791e64f 100644 --- a/src/src.ino +++ b/src/src.ino @@ -31,11 +31,18 @@ struct CamInfosGlue { Optional 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 bP; - if (ball_x != 0 && ball_y != 0) { - bP = BallPos(ball_x, ball_y); + if (ballX != 0 && ballY != 0) { + bP = BallPos(ballX, ballY); } Optional 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 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 index 9433485..9c78279 100644 --- 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());