From 311c4620365ebd4eda6f720d880c82058773e277 Mon Sep 17 00:00:00 2001 From: Ricardo Date: Sun, 1 Feb 2026 16:51:38 +0000 Subject: [PATCH] Working on Phase 1 --- Firmware/micro_motors/micro_motors.ino | 134 ++++++++++++++++--------- 1 file changed, 88 insertions(+), 46 deletions(-) diff --git a/Firmware/micro_motors/micro_motors.ino b/Firmware/micro_motors/micro_motors.ino index ee51f8e..fe8d19d 100644 --- a/Firmware/micro_motors/micro_motors.ino +++ b/Firmware/micro_motors/micro_motors.ino @@ -49,7 +49,9 @@ const char* password = "hi123456"; int trimFL = 0; int trimFR = 0; int trimBL = 0; - int trimBR = 0; + int trimBR = 0; + +bool ph1 = false; // Phase 1 complete flag // Telnet server to watch serial @@ -57,14 +59,14 @@ WiFiServer telnetServer(23); WiFiClient telnetClient; // Function declaritons -void otaTask(void *parameter); // Get micro ready for OTA -void logPrint(const char* msg); // Print function for serial and telnet, strings -void logPrint(int val); // Print function for serial and telnet, integers -void logPrint(float val); // Print function for serial and telnet, floats -void logPrintln(const char* msg); // Print line function for serial and telnet -void logPrintln(int val); // Print line function for serial and telnet, integers -void logPrintln(float val); // Print line function for serial and telnet, floats -void telnetTask(void *parameter); // Configure telnet serial +void otaTask(void *parameter); // Get micro ready for OTA +void logPrint(const char* msg); // Print function for serial and telnet, strings +void logPrint(int val); // Print function for serial and telnet, integers +void logPrint(float val); // Print function for serial and telnet, floats +void logPrintln(const char* msg); // Print line function for serial and telnet +void logPrintln(int val); // Print line function for serial and telnet, integers +void logPrintln(float val); // Print line function for serial and telnet, floats +void telnetTask(void *parameter); // Configure telnet serial void motorFR(int speed); // Front right motor, speed from -255 to 255, positive goes forward, negative goes backward void motorFL(int speed); // Front left motor, speed from -255 to 255, positive goes forward, negative goes backward void motorBR(int speed); // Back right motor, speed from -255 to 255, positive goes forward, negative goes backward @@ -155,28 +157,62 @@ if (WiFi.status() == WL_CONNECTED) { if (WiFi.status() == WL_CONNECTED) { xTaskCreate(otaTask, "OTA", 4096, NULL, 1, NULL); xTaskCreate(telnetTask, "Telnet", 4096, NULL, 1, NULL); - } + } } void loop() { +// ph1 = true; -receiveSensorData(); -while ((sens_FL + sens_FR) / 2 > 174) { - motorsGo(180); - receiveSensorData(); -} - -motorsStop(); -delay(1000); -Go90right(); + if (!ph1) { + phaseOne(); + ph1 = true; + } // motorsTest(); // sensorsTest(); } +void phaseOne() { + // Wait for valid sensor readings + while (sens_FL < 0 || sens_FR < 0) { + receiveSensorData(); // read sensors + delay(10); + } + + int distance; + int speed; + + receiveSensorData(); + while ((distance = (sens_FL + sens_FR) / 2) > 250) { + // Slow down as we approach target + if (distance > 500) { + speed = 180; + } else if (distance > 400) { + speed = 120; + } else { + speed = 80; // Slow down even more + } + motorsGo(speed); + delay(5); // Wait for new sensor data to arrive + receiveSensorData(); // Get fresh reading each iteration + } + + motorsStop(); + logPrint("Stopped at: "); logPrintln((sens_FL + sens_FR) / 2); + delay(1000); + Go90right(); + delay(2000); + motorsGo(80); + delay(1650); // Go into the pit 1800 miliseconds + motorsStop(); + delay(3000); + motorsGo(-150); + delay(1600); // Go out of the pit 1800 miliseconds + motorsStop(); +} void otaTask(void *parameter) { for (;;) { @@ -196,7 +232,7 @@ void telnetTask(void *parameter) { } vTaskDelay(100 / portTICK_PERIOD_MS); } - } + } void logPrint(const char* msg) { Serial.print(msg); @@ -311,30 +347,36 @@ void motorBR(int speed) { motorBL(0); } - void receiveSensorData() { - if (Serial2.available()) { - String data = Serial2.readStringUntil('\n'); - int values[8]; - int idx = 0; - int start = 0; - - for (int i = 0; i <= data.length() && idx < 8; i++) { - if (i == data.length() || data[i] == ',') { - values[idx++] = data.substring(start, i).toInt(); - start = i + 1; - } - } - - if (idx == 8) { - sens_FL = values[0]; - sens_FR = values[1]; - sens_BL = values[2]; - sens_BR = values[3]; - sens_LF = values[4]; - sens_LB = values[5]; - sens_RF = values[6]; - sens_RB = values[7]; - } + void receiveSensorData() { + String data; + + // Drain buffer - keep only the latest complete line + while (Serial2.available()) { + data = Serial2.readStringUntil('\n'); + } + + if (data.length() == 0) return; + + int values[8]; + int idx = 0; + int start = 0; + + for (int i = 0; i <= data.length() && idx < 8; i++) { + if (i == data.length() || data[i] == ',') { + values[idx++] = data.substring(start, i).toInt(); + start = i + 1; + } + } + + if (idx == 8) { + sens_FL = values[0]; + sens_FR = values[1]; + sens_BL = values[2]; + sens_BR = values[3]; + sens_LF = values[4]; + sens_LB = values[5]; + sens_RF = values[6]; + sens_RB = values[7]; } } @@ -461,7 +503,7 @@ void Go90right() { motorFR(-150); motorBL(150); motorBR(-150); - delay(1735); + delay(1150); motorsStop(); } @@ -470,7 +512,7 @@ void Go90left() { motorFR(150); motorBL(-150); motorBR(150); - delay(1735); + delay(1150); motorsStop(); }