RobotoLab Day2

team-raiju · Mar 20, 2022 · 50706dd · 50706dd
1 parent 6eef55f
commit 50706dd
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 35 deletions.
diff --git a/src/controllers/RCState.cpp b/src/controllers/RCState.cpp
@@ -37,18 +37,15 @@ void FSM::RCState::cycle(FSM* fsm) {
     auto mot1 = coords.y + coords.x;
     auto mot2 = coords.y - coords.x;
 
-    fsm->s_driving.drive(mot1, mot2);
-
-    if (fsm->s_bt.data_available()) {
-        auto packet = fsm->s_bt.last_read_packet();
-
-        if (packet.byte1 == 0x01) {
-            if (packet.byte2 == 0x02) {
-                fsm->set_state(IdleState::instance());
-                return;
-            }
+    if (fsm->s_radio.get_ch3() > 1500) {
+        if (fsm->s_line.is_white(LineService::Position::FR1) || fsm->s_line.is_white(LineService::Position::FL1)) {
+            mot1 = mot2 = -100;
+        } else if (fsm->s_line.is_white(LineService::Position::BR) || fsm->s_line.is_white(LineService::Position::BL)) {
+            mot1 = mot2 = 100;
         }
     }
+
+    fsm->s_driving.drive(mot1, mot2);
 }
 
 void FSM::RCState::exit(FSM* fsm) {

diff --git a/src/controllers/StrategyState.cpp b/src/controllers/StrategyState.cpp
@@ -57,39 +57,39 @@ void FSM::StrategyState::cycle(FSM* fsm) {
     if (fsm->strategy != 1) {
         if (fsm->s_line.is_white(LineService::Position::FR1) || fsm->s_line.is_white(LineService::Position::FL1)) {
             fsm->s_driving.drive(-100, -100);
-            mcu::sleep(100); // TODO: Calibração desses valores de tempo de volta pras curvas
+            mcu::sleep(150); // TODO: Calibração desses valores de tempo de volta pras curvas
 
             int8_t mult = fsm->s_line.is_white(LineService::Position::FR1) ? -1 : 1;
-            fsm->s_driving.drive(75 * mult, -75 * mult);
-            mcu::sleep(100);
+            fsm->s_driving.drive(100 * mult, -100 * mult);
+            mcu::sleep(175);
         }
     }
 
-    if (fsm->strategy == 2) {
+    if (fsm->s_distance.is_reading(DIST_FRONT) ||
+        (fsm->s_distance.is_reading(DIST_BRIGHT) && fsm->s_distance.is_reading(DIST_BLEFT)) || fsm->strategy == 2) {
         fsm->s_driving.drive(100, 100);
+        still = false;
+    } else if (fsm->s_distance.is_reading(DIST_BRIGHT)) {
+        fsm->s_driving.drive(100, 30);
+        still = false;
+    } else if (fsm->s_distance.is_reading(DIST_BLEFT)) {
+        fsm->s_driving.drive(30, 100);
+        still = false;
+    } else if (fsm->s_distance.is_reading(DIST_RIGHT)) {
+        fsm->s_driving.drive(100, -100);
+        still = false;
+    } else if (fsm->s_distance.is_reading(DIST_LEFT)) {
+        fsm->s_driving.drive(-100, 100);
+        still = false;
+    } else if (fsm->strategy == 4) {
+        fsm->s_driving.drive(100, 100);
+        still = false;
     } else {
-        if (fsm->s_distance.is_reading(DIST_FRONT) || fsm->strategy == 4) {
-            fsm->s_driving.drive(100, 100);
-            still = false;
-        } else if (fsm->s_distance.is_reading(DIST_BRIGHT)) {
-            fsm->s_driving.drive(100, 30);
-            still = false;
-        } else if (fsm->s_distance.is_reading(DIST_BLEFT)) {
-            fsm->s_driving.drive(30, 100);
-            still = false;
-        } else if (fsm->s_distance.is_reading(DIST_RIGHT)) {
-            fsm->s_driving.drive(100, -100);
-            still = false;
-        } else if (fsm->s_distance.is_reading(DIST_LEFT)) {
-            fsm->s_driving.drive(-100, 100);
-            still = false;
-        } else {
-            if (!still) {
-                still = true;
-                ticker = mcu::get_tick();
-            }
-            fsm->s_driving.drive(0, 0);
+        if (!still) {
+            still = true;
+            ticker = mcu::get_tick();
         }
+        fsm->s_driving.drive(0, 0);
     }
 
     if (still && mcu::get_tick() - ticker >= 3000) {