diff --git a/Firmware/micro_motors/micro_motors.ino b/Firmware/micro_motors/micro_motors.ino index fe8d19d..219e366 100644 --- a/Firmware/micro_motors/micro_motors.ino +++ b/Firmware/micro_motors/micro_motors.ino @@ -1,531 +1,614 @@ -#include -#include - - -const char* ssid = "CIA"; -const char* password = "hi123456"; - -#define motorAttach ledcAttach // Get more appropriate names -#define motorSpeed ledcWrite // same " - -// Motor FR (Front Right) - #define FR_IN1 27 - #define FR_IN2 14 - #define FR_PWM 0 - - // Motor FL (Front Left) - #define FL_IN1 12 - #define FL_IN2 13 - #define FL_PWM 2 - -// Motor BR (Back Right) - #define BR_IN1 33 - #define BR_IN2 32 - #define BR_PWM 23 - - // Motor BL (Back Left) - #define BL_IN1 26 - #define BL_IN2 25 - #define BL_PWM 22 - - - // PWM settings - #define PWM_FREQ 1000 - #define PWM_RESOLUTION 8 // 0-255 - - #define SERIAL_RX 21 - - // Sensor data received from micro sensors - int sens_FL = -1; - int sens_FR = -1; - int sens_BL = -1; - int sens_BR = -1; - int sens_LF = -1; - int sens_LB = -1; - int sens_RF = -1; - int sens_RB = -1; - - // Trim values to calibrate wheel speeds - int trimFL = 0; - int trimFR = 0; - int trimBL = 0; - int trimBR = 0; - -bool ph1 = false; // Phase 1 complete flag - - -// Telnet server to watch serial -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 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 -void motorBL(int speed); // Back left motor, speed from -255 to 255, positive goes forward, negative goes backward -void motorsStop(); // Stop all motors -void motorsGo(int speed); // All motors move the same speed, from -255 to 255, positive goes forward, negative goes backward -void Go45right(); -void Go45left(); -void Go90right(); -void Go90left(); -void slideRight(); -void slideLeft(); -void receiveSensorData(); // Receive the sensors data from micro sensors -void sensorsTest(); -void motorsTest(); - - -void setup() { - Serial.begin(115200); - - // Configure WIFI - WiFi.mode(WIFI_STA); - WiFi.begin(ssid, password); - - int wifiAttempts = 0; - while (WiFi.waitForConnectResult() != WL_CONNECTED && wifiAttempts < 10) { - Serial.println("WiFi connecting..."); - wifiAttempts++; - delay(500); - } - - if (WiFi.status() == WL_CONNECTED) { - Serial.print("IP Address: "); - Serial.println(WiFi.localIP()); - } else { - Serial.println("WiFi not available - running without network"); - } - - -if (WiFi.status() == WL_CONNECTED) { - ArduinoOTA.setHostname("Motors"); - ArduinoOTA.onStart([]() { - Serial.println("OTA Update starting..."); - }); - ArduinoOTA.onEnd([]() { - Serial.println("\nOTA Update complete!"); - }); - ArduinoOTA.onError([](ota_error_t error) { - Serial.printf("OTA Error [%u]\n", error); - }); - ArduinoOTA.begin(); - telnetServer.begin(); - } - - Serial2.begin(115200, SERIAL_8N1, SERIAL_RX, -1); - - // Motor direction pins - pinMode(FR_IN1, OUTPUT); - pinMode(FR_IN2, OUTPUT); - pinMode(FL_IN1, OUTPUT); - pinMode(FL_IN2, OUTPUT); - pinMode(BR_IN1, OUTPUT); - pinMode(BR_IN2, OUTPUT); - pinMode(BL_IN1, OUTPUT); - pinMode(BL_IN2, OUTPUT); - - // PWM setup for ESP32 - motorAttach(FR_PWM, PWM_FREQ, PWM_RESOLUTION); - motorAttach(FL_PWM, PWM_FREQ, PWM_RESOLUTION); - motorAttach(BR_PWM, PWM_FREQ, PWM_RESOLUTION); - motorAttach(BL_PWM, PWM_FREQ, PWM_RESOLUTION); - - // Start with motors stopped - digitalWrite(FR_IN1, LOW); - digitalWrite(FR_IN2, LOW); - digitalWrite(FL_IN1, LOW); - digitalWrite(FL_IN2, LOW); - motorSpeed(FR_PWM, 0); - motorSpeed(FL_PWM, 0); - digitalWrite(BR_IN1, LOW); - digitalWrite(BR_IN2, LOW); - digitalWrite(BL_IN1, LOW); - digitalWrite(BL_IN2, LOW); - motorSpeed(BR_PWM, 0); - motorSpeed(BL_PWM, 0); - - // FreeRTOS tasks - if (WiFi.status() == WL_CONNECTED) { - xTaskCreate(otaTask, "OTA", 4096, NULL, 1, NULL); - xTaskCreate(telnetTask, "Telnet", 4096, NULL, 1, NULL); - } - - -} - -void loop() { -// ph1 = true; - - 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 (;;) { - ArduinoOTA.handle(); - vTaskDelay(10 / portTICK_PERIOD_MS); - } - } - -void telnetTask(void *parameter) { - for (;;) { - if (telnetServer.hasClient()) { - if (telnetClient && telnetClient.connected()) { - telnetClient.stop(); - } - telnetClient = telnetServer.available(); - telnetClient.println("Connected to Motors"); - } - vTaskDelay(100 / portTICK_PERIOD_MS); - } - } - - void logPrint(const char* msg) { - Serial.print(msg); - if (telnetClient && telnetClient.connected()) { - telnetClient.print(msg); - } - } - - void logPrint(int val) { - Serial.print(val); - if (telnetClient && telnetClient.connected()) { - telnetClient.print(val); - } - } - - void logPrint(float val) { - Serial.print(val); - if (telnetClient && telnetClient.connected()) { - telnetClient.print(val); - } - } - - void logPrintln(const char* msg) { - Serial.println(msg); - if (telnetClient && telnetClient.connected()) { - telnetClient.println(msg); - } - } - - void logPrintln(int val) { - Serial.println(val); - if (telnetClient && telnetClient.connected()) { - telnetClient.println(val); - } - } - - void logPrintln(float val) { - Serial.println(val); - if (telnetClient && telnetClient.connected()) { - telnetClient.println(val); - } - } - -void motorFR(int speed) { - if (speed > 0) { - digitalWrite(FR_IN1, HIGH); - digitalWrite(FR_IN2, LOW); - motorSpeed(FR_PWM, constrain(speed + trimFR, 0, 255)); - } else if (speed < 0) { - digitalWrite(FR_IN1, LOW); - digitalWrite(FR_IN2, HIGH); - motorSpeed(FR_PWM, constrain(-speed + trimFR, 0, 255)); - } else { - digitalWrite(FR_IN1, LOW); - digitalWrite(FR_IN2, LOW); - motorSpeed(FR_PWM, 0); - } - } - - void motorFL(int speed) { - if (speed > 0) { - digitalWrite(FL_IN1, HIGH); - digitalWrite(FL_IN2, LOW); - motorSpeed(FL_PWM, constrain(speed + trimFL, 0, 255)); - } else if (speed < 0) { - digitalWrite(FL_IN1, LOW); - digitalWrite(FL_IN2, HIGH); - motorSpeed(FL_PWM, constrain(-speed + trimFL, 0, 255)); - } else { - digitalWrite(FL_IN1, LOW); - digitalWrite(FL_IN2, LOW); - motorSpeed(FL_PWM, 0); - } - } - -void motorBR(int speed) { - if (speed > 0) { - digitalWrite(BR_IN1, HIGH); - digitalWrite(BR_IN2, LOW); - motorSpeed(BR_PWM, constrain(speed + trimBR, 0, 255)); - } else if (speed < 0) { - digitalWrite(BR_IN1, LOW); - digitalWrite(BR_IN2, HIGH); - motorSpeed(BR_PWM, constrain(-speed + trimBR, 0, 255)); - } else { - digitalWrite(BR_IN1, LOW); - digitalWrite(BR_IN2, LOW); - motorSpeed(BR_PWM, 0); - } - } - - void motorBL(int speed) { - if (speed > 0) { - digitalWrite(BL_IN1, HIGH); - digitalWrite(BL_IN2, LOW); - motorSpeed(BL_PWM, constrain(speed + trimBL, 0, 255)); - } else if (speed < 0) { - digitalWrite(BL_IN1, LOW); - digitalWrite(BL_IN2, HIGH); - motorSpeed(BL_PWM, constrain(-speed + trimBL, 0, 255)); - } else { - digitalWrite(BL_IN1, LOW); - digitalWrite(BL_IN2, LOW); - motorSpeed(BL_PWM, 0); - } - } - - void motorsStop() { - motorFR(0); - motorFL(0); - motorBR(0); - motorBL(0); - } - - 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]; - } - } - -void sensorsTest() { - - receiveSensorData(); - - logPrint("FL:"); logPrint(sens_FL); - logPrint(" FR:"); logPrint(sens_FR); - logPrint(" BL:"); logPrint(sens_BL); - logPrint(" BR:"); logPrint(sens_BR); - logPrint(" LF:"); logPrint(sens_LF); - logPrint(" LB:"); logPrint(sens_LB); - logPrint(" RF:"); logPrint(sens_RF); - logPrint(" RB:"); logPrintln(sens_RB); - - delay(100); -} - -void motorsTest() { - - logPrintln("Right forward..."); - motorBR(80); - motorFR(80); - delay(2000); - - logPrintln("Right stop..."); - motorBR(0); - motorFR(0); - delay(1000); - - logPrintln("Right forward..."); - motorBR(250); - motorFR(250); - delay(2000); - - logPrintln("Right stop..."); - motorBR(0); - motorFR(0); - delay(1000); - - logPrintln("Left forward..."); - motorBL(80); - motorFL(80); - delay(2000); - - logPrintln("Left stop..."); - motorsStop(); - delay(1000); - - logPrintln("Left forward..."); - motorBL(250); - motorFL(250); - delay(2000); - - logPrintln("Left stop..."); - motorsStop(); - delay(1000); - - logPrintln("Front forward..."); - motorFR(80); - motorFL(80); - delay(2000); - - logPrintln("Front stop..."); - motorsStop(); - delay(1000); - - logPrintln("Front forward..."); - motorFR(250); - motorFL(250); - delay(2000); - - logPrintln("Front stop..."); - motorsStop(); - delay(1000); - - logPrintln("Back forward..."); - motorBR(80); - motorBL(80); - delay(2000); - - logPrintln("Back stop..."); - motorsStop(); - delay(1000); - - logPrintln("Back forward..."); - motorBR(250); - motorBL(250); - delay(2000); - - logPrintln("Back stop..."); - motorsStop(); - delay(1000); -} - -void motorsGo(int speed) { - motorFR(speed); - motorFL(speed); - motorBR(speed); - motorBL(speed); -} - -void Go45right() { - motorFL(150); - motorFR(-150); - motorBL(150); - motorBR(-150); - delay(875); - motorsStop(); -} - -void Go45left() { - motorFL(-150); - motorFR(150); - motorBL(-150); - motorBR(150); - delay(875); - motorsStop(); -} - -void Go90right() { - motorFL(150); - motorFR(-150); - motorBL(150); - motorBR(-150); - delay(1150); - motorsStop(); -} - -void Go90left() { - motorFL(-150); - motorFR(150); - motorBL(-150); - motorBR(150); - delay(1150); - motorsStop(); -} - -void slideRight() { - motorFL(-150); - motorFR(150); - motorBL(150); - motorBR(-150); -} - -void slideLeft() { - motorFL(150); - motorFR(-150); - motorBL(-150); - motorBR(150); -} +#include +#include + + +const char* ssid = "CIA"; +const char* password = "hi123456"; + +#define motorAttach ledcAttach // Get more appropriate names +#define motorSpeed ledcWrite // same " + +// Motor FR (Front Right) + #define FR_IN1 27 + #define FR_IN2 14 + #define FR_PWM 0 + + // Motor FL (Front Left) + #define FL_IN1 12 + #define FL_IN2 13 + #define FL_PWM 2 + +// Motor BR (Back Right) + #define BR_IN1 33 + #define BR_IN2 32 + #define BR_PWM 23 + + // Motor BL (Back Left) + #define BL_IN1 26 + #define BL_IN2 25 + #define BL_PWM 22 + + + // PWM settings + #define PWM_FREQ 1000 + #define PWM_RESOLUTION 8 // 0-255 + + #define SERIAL_RX 21 + + // Sensor data received from micro sensors + int sens_FL = -1; + int sens_FR = -1; + int sens_BL = -1; + int sens_BR = -1; + int sens_LF = -1; + int sens_LB = -1; + int sens_RF = -1; + int sens_RB = -1; + + // Trim values to calibrate wheel speeds + int trimFL = 0; + int trimFR = 0; + int trimBL = 0; + int trimBR = 0; + #define BUFFER_SIZE 7 + int buffer_FL[BUFFER_SIZE]; + int buffer_FR[BUFFER_SIZE]; + int buffer_BL[BUFFER_SIZE]; + int buffer_BR[BUFFER_SIZE]; + int buffer_LF[BUFFER_SIZE]; + int buffer_LB[BUFFER_SIZE]; + int buffer_RF[BUFFER_SIZE]; + int buffer_RB[BUFFER_SIZE]; + + int bufferIndex = 0; + bool bufferFilled = false; + bool ph1 = false; // Phase 1 complete flag + bool ph2 = false; //phase 2 complete flag + bool ph3 = false; //phase 3 complete flag + bool p2_stop = false; // Flag when its passed the top of the hill + +// Telnet server to watch serial +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 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 +void motorBL(int speed); // Back left motor, speed from -255 to 255, positive goes forward, negative goes backward +void motorsStop(); // Stop all motors +void motorsGo(int speed); // All motors move the same speed, from -255 to 255, positive goes forward, negative goes backward +void Go45right(); +void Go45left(); +void Go90right(); +void Go90left(); +void slideRight(); +void slideLeft(); +void receiveSensorData(); // Receive the sensors data from micro sensors +int rollingAverage(int buffer[]); // rollingAverage function call +void sensorsTest(); +void motorsTest(); +void phaseThree(); + +void setup() { + Serial.begin(115200); + + // Configure WIFI + WiFi.mode(WIFI_STA); + WiFi.begin(ssid, password); + + int wifiAttempts = 0; + while (WiFi.waitForConnectResult() != WL_CONNECTED && wifiAttempts < 10) { + Serial.println("WiFi connecting..."); + wifiAttempts++; + delay(500); + } + + if (WiFi.status() == WL_CONNECTED) { + Serial.print("IP Address: "); + Serial.println(WiFi.localIP()); + } else { + Serial.println("WiFi not available - running without network"); + } + + +if (WiFi.status() == WL_CONNECTED) { + ArduinoOTA.setHostname("Motors"); + ArduinoOTA.onStart([]() { + Serial.println("OTA Update starting..."); + }); + ArduinoOTA.onEnd([]() { + Serial.println("\nOTA Update complete!"); + }); + ArduinoOTA.onError([](ota_error_t error) { + Serial.printf("OTA Error [%u]\n", error); + }); + ArduinoOTA.begin(); + telnetServer.begin(); + } + + Serial2.begin(115200, SERIAL_8N1, SERIAL_RX, -1); + + // Motor direction pins + pinMode(FR_IN1, OUTPUT); + pinMode(FR_IN2, OUTPUT); + pinMode(FL_IN1, OUTPUT); + pinMode(FL_IN2, OUTPUT); + pinMode(BR_IN1, OUTPUT); + pinMode(BR_IN2, OUTPUT); + pinMode(BL_IN1, OUTPUT); + pinMode(BL_IN2, OUTPUT); + + // PWM setup for ESP32 + motorAttach(FR_PWM, PWM_FREQ, PWM_RESOLUTION); + motorAttach(FL_PWM, PWM_FREQ, PWM_RESOLUTION); + motorAttach(BR_PWM, PWM_FREQ, PWM_RESOLUTION); + motorAttach(BL_PWM, PWM_FREQ, PWM_RESOLUTION); + + // Start with motors stopped + digitalWrite(FR_IN1, LOW); + digitalWrite(FR_IN2, LOW); + digitalWrite(FL_IN1, LOW); + digitalWrite(FL_IN2, LOW); + motorSpeed(FR_PWM, 0); + motorSpeed(FL_PWM, 0); + digitalWrite(BR_IN1, LOW); + digitalWrite(BR_IN2, LOW); + digitalWrite(BL_IN1, LOW); + digitalWrite(BL_IN2, LOW); + motorSpeed(BR_PWM, 0); + motorSpeed(BL_PWM, 0); + + // FreeRTOS tasks + if (WiFi.status() == WL_CONNECTED) { + xTaskCreate(otaTask, "OTA", 4096, NULL, 1, NULL); + xTaskCreate(telnetTask, "Telnet", 4096, NULL, 1, NULL); + } + + +} + +void loop() { +// ph1 = true; +// ph2 = true; +// ph3 = true; + + + if (!ph1) { + phaseOne(); + ph1 = true; + } + + if (!ph2) { + phaseTwo(); + ph2 = true; + } + + if (!ph3) { + phaseThree(); + ph3 = 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) > 240) { + // Slow down as we approach target + if (distance > 500) { + speed = 180; + } else if (distance > 350) { + speed = 80; + } else { + speed = 40; // 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(1250); // Go into the pit 1800 miliseconds, + motorsStop(); + delay(3000); //how long to pick balls? dunno + motorsGo(-150); + delay(1200); // Go out of the pit 1800 miliseconds + motorsStop(); + delay(500); + slideLeft(); + delay(1450); + motorsStop(); + delay(2000); +} + +void phaseTwo() { + // Wait for valid sensor readings - ENABLE IF YOU ARE TESTING ONLY THIS PHASE +// while (sens_FL < 0 || sens_FR < 0) { +// receiveSensorData(); // read sensors +// delay(10); +// } + + receiveSensorData(); + + + while (!p2_stop) { + while ((sens_FL + sens_FR) / 2 > 8000) { + motorsGo(-255); + delay(5); // Wait for new sensor data to arrive + receiveSensorData(); // Get fresh reading each iteration + } + while ((sens_FL + sens_FR) / 2 < 680) { // Used to be 680 + motorsGo(-180); + delay(5); // Wait for new sensor data to arrive + receiveSensorData(); // Get fresh reading each iteration + } + while ((sens_FL + sens_FR) / 2 > 320) { + motorsGo(-120); + delay(5); // Wait for new sensor data to arrive + receiveSensorData(); // Get fresh reading each iteration + } + p2_stop = true; + receiveSensorData(); + } + motorsStop(); + + + +} + +void phaseThree() { + + +} + +void otaTask(void *parameter) { + for (;;) { + ArduinoOTA.handle(); + vTaskDelay(10 / portTICK_PERIOD_MS); + } + } + +void telnetTask(void *parameter) { + for (;;) { + if (telnetServer.hasClient()) { + if (telnetClient && telnetClient.connected()) { + telnetClient.stop(); + } + telnetClient = telnetServer.available(); + telnetClient.println("Connected to Motors"); + } + vTaskDelay(100 / portTICK_PERIOD_MS); + } + } + + void logPrint(const char* msg) { + Serial.print(msg); + if (telnetClient && telnetClient.connected()) { + telnetClient.print(msg); + } + } + + void logPrint(int val) { + Serial.print(val); + if (telnetClient && telnetClient.connected()) { + telnetClient.print(val); + } + } + + void logPrint(float val) { + Serial.print(val); + if (telnetClient && telnetClient.connected()) { + telnetClient.print(val); + } + } + + void logPrintln(const char* msg) { + Serial.println(msg); + if (telnetClient && telnetClient.connected()) { + telnetClient.println(msg); + } + } + + void logPrintln(int val) { + Serial.println(val); + if (telnetClient && telnetClient.connected()) { + telnetClient.println(val); + } + } + + void logPrintln(float val) { + Serial.println(val); + if (telnetClient && telnetClient.connected()) { + telnetClient.println(val); + } + } + +void motorFR(int speed) { + if (speed > 0) { + digitalWrite(FR_IN1, HIGH); + digitalWrite(FR_IN2, LOW); + motorSpeed(FR_PWM, constrain(speed + trimFR, 0, 255)); + } else if (speed < 0) { + digitalWrite(FR_IN1, LOW); + digitalWrite(FR_IN2, HIGH); + motorSpeed(FR_PWM, constrain(-speed + trimFR, 0, 255)); + } else { + digitalWrite(FR_IN1, LOW); + digitalWrite(FR_IN2, LOW); + motorSpeed(FR_PWM, 0); + } + } + + void motorFL(int speed) { + if (speed > 0) { + digitalWrite(FL_IN1, HIGH); + digitalWrite(FL_IN2, LOW); + motorSpeed(FL_PWM, constrain(speed + trimFL, 0, 255)); + } else if (speed < 0) { + digitalWrite(FL_IN1, LOW); + digitalWrite(FL_IN2, HIGH); + motorSpeed(FL_PWM, constrain(-speed + trimFL, 0, 255)); + } else { + digitalWrite(FL_IN1, LOW); + digitalWrite(FL_IN2, LOW); + motorSpeed(FL_PWM, 0); + } + } + +void motorBR(int speed) { + if (speed > 0) { + digitalWrite(BR_IN1, HIGH); + digitalWrite(BR_IN2, LOW); + motorSpeed(BR_PWM, constrain(speed + trimBR, 0, 255)); + } else if (speed < 0) { + digitalWrite(BR_IN1, LOW); + digitalWrite(BR_IN2, HIGH); + motorSpeed(BR_PWM, constrain(-speed + trimBR, 0, 255)); + } else { + digitalWrite(BR_IN1, LOW); + digitalWrite(BR_IN2, LOW); + motorSpeed(BR_PWM, 0); + } + } + + void motorBL(int speed) { + if (speed > 0) { + digitalWrite(BL_IN1, HIGH); + digitalWrite(BL_IN2, LOW); + motorSpeed(BL_PWM, constrain(speed + trimBL, 0, 255)); + } else if (speed < 0) { + digitalWrite(BL_IN1, LOW); + digitalWrite(BL_IN2, HIGH); + motorSpeed(BL_PWM, constrain(-speed + trimBL, 0, 255)); + } else { + digitalWrite(BL_IN1, LOW); + digitalWrite(BL_IN2, LOW); + motorSpeed(BL_PWM, 0); + } + } + + void motorsStop() { + motorFR(0); + motorFL(0); + motorBR(0); + motorBL(0); + } + + int rollingAverage(int buffer[]) { + int sum = 0; + int count = bufferFilled ? BUFFER_SIZE : (bufferIndex + 1); + + for (int i = 0; i < count; i++) { + sum += buffer[i]; + } + + return sum / count; +} + + 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]; + } + } + +void sensorsTest() { + + receiveSensorData(); + + logPrint("FL:"); logPrint(sens_FL); + logPrint(" FR:"); logPrint(sens_FR); + logPrint(" BL:"); logPrint(sens_BL); + logPrint(" BR:"); logPrint(sens_BR); + logPrint(" LF:"); logPrint(sens_LF); + logPrint(" LB:"); logPrint(sens_LB); + logPrint(" RF:"); logPrint(sens_RF); + logPrint(" RB:"); logPrintln(sens_RB); + + delay(100); +} + +void motorsTest() { + + logPrintln("Right forward..."); + motorBR(80); + motorFR(80); + delay(2000); + + logPrintln("Right stop..."); + motorBR(0); + motorFR(0); + delay(1000); + + logPrintln("Right forward..."); + motorBR(250); + motorFR(250); + delay(2000); + + logPrintln("Right stop..."); + motorBR(0); + motorFR(0); + delay(1000); + + logPrintln("Left forward..."); + motorBL(80); + motorFL(80); + delay(2000); + + logPrintln("Left stop..."); + motorsStop(); + delay(1000); + + logPrintln("Left forward..."); + motorBL(250); + motorFL(250); + delay(2000); + + logPrintln("Left stop..."); + motorsStop(); + delay(1000); + + logPrintln("Front forward..."); + motorFR(80); + motorFL(80); + delay(2000); + + logPrintln("Front stop..."); + motorsStop(); + delay(1000); + + logPrintln("Front forward..."); + motorFR(250); + motorFL(250); + delay(2000); + + logPrintln("Front stop..."); + motorsStop(); + delay(1000); + + logPrintln("Back forward..."); + motorBR(80); + motorBL(80); + delay(2000); + + logPrintln("Back stop..."); + motorsStop(); + delay(1000); + + logPrintln("Back forward..."); + motorBR(250); + motorBL(250); + delay(2000); + + logPrintln("Back stop..."); + motorsStop(); + delay(1000); +} + +void motorsGo(int speed) { + motorFR(speed); + motorFL(speed); + motorBR(speed); + motorBL(speed); +} + +void Go45right() { + motorFL(150); + motorFR(-150); + motorBL(150); + motorBR(-150); + delay(875); + motorsStop(); +} + +void Go45left() { + motorFL(-150); + motorFR(150); + motorBL(-150); + motorBR(150); + delay(875); + motorsStop(); +} + +void Go90right() { + motorFL(150); + motorFR(-150); + motorBL(150); + motorBR(-150); + delay(1250); + motorsStop(); +} + +void Go90left() { + motorFL(-150); + motorFR(150); + motorBL(-150); + motorBR(150); + delay(1250); + motorsStop(); +} + +void slideRight() { + motorFL(-150); + motorFR(150); + motorBL(150); + motorBR(-150); +} + +void slideLeft() { + motorFL(150); + motorFR(-150); + motorBL(-150); + motorBR(150); +}