diff --git a/Firmware/micro_motors/micro_motors.ino b/Firmware/micro_motors/micro_motors.ino new file mode 100644 index 0000000..bde83c7 --- /dev/null +++ b/Firmware/micro_motors/micro_motors.ino @@ -0,0 +1,481 @@ +#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; + + +// 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() { + + + +// motorsTest(); + sensorsTest(); + +} + + +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() { + 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 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(1735); + motorsStop(); +} + +void Go90left() { + motorFL(-150); + motorFR(150); + motorBL(-150); + motorBR(150); + delay(1735); + motorsStop(); +} + +void slideRight() { + motorFL(-150); + motorFR(150); + motorBL(150); + motorBR(-150); +} + +void slideLeft() { + motorFL(150); + motorFR(-150); + motorBL(-150); + motorBR(150); +} diff --git a/Firmware/micro_sensors/micro_sensors.ino b/Firmware/micro_sensors/micro_sensors.ino new file mode 100644 index 0000000..4a0497b --- /dev/null +++ b/Firmware/micro_sensors/micro_sensors.ino @@ -0,0 +1,465 @@ +#include +#include +#include "Adafruit_VL53L0X.h" + +// Sensors I2C addresses +#define SFL_ADDRESS 0x30 +#define SFR_ADDRESS 0x31 +#define SBL_ADDRESS 0x32 +#define SBR_ADDRESS 0x33 +#define SLF_ADDRESS 0x34 +#define SLB_ADDRESS 0x35 +#define SRF_ADDRESS 0x36 +#define SRB_ADDRESS 0x37 + +// Sensors shutdown pins +#define SHT_SFL 19 +#define SHT_SFR 18 +#define SHT_SBL 16 +#define SHT_SBR 4 +#define SHT_SLF 0 +#define SHT_SLB 2 +#define SHT_SRF 17 +#define SHT_SRB 5 + +#define SERIAL_TX 15 + +// Sensors objects +Adafruit_VL53L0X SFL = Adafruit_VL53L0X(); +Adafruit_VL53L0X SFR = Adafruit_VL53L0X(); +Adafruit_VL53L0X SBL = Adafruit_VL53L0X(); +Adafruit_VL53L0X SBR = Adafruit_VL53L0X(); +Adafruit_VL53L0X SLF = Adafruit_VL53L0X(); +Adafruit_VL53L0X SLB = Adafruit_VL53L0X(); +Adafruit_VL53L0X SRF = Adafruit_VL53L0X(); +Adafruit_VL53L0X SRB = Adafruit_VL53L0X(); + +// this holds the measurement +VL53L0X_RangingMeasurementData_t m_FL; +VL53L0X_RangingMeasurementData_t m_FR; +VL53L0X_RangingMeasurementData_t m_BL; +VL53L0X_RangingMeasurementData_t m_BR; +VL53L0X_RangingMeasurementData_t m_LF; +VL53L0X_RangingMeasurementData_t m_LB; +VL53L0X_RangingMeasurementData_t m_RF; +VL53L0X_RangingMeasurementData_t m_RB; + +// Track which sensors initialized + bool SFL_OK = false; + bool SFR_OK = false; + bool SBL_OK = false; + bool SBR_OK = false; + bool SLF_OK = false; + bool SLB_OK = false; + bool SRF_OK = false; + bool SRB_OK = false; + + +const char* ssid = "CIA"; +const char* password = "hi123456"; + +// 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 setID(); // Set I2C on sensors +void read_sensors(); // Read sensors test +void sendSensorData(); // Send sensors data to micro motors + +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("Sensors"); + 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, -1, SERIAL_TX); + + pinMode(SHT_SFL, OUTPUT); + pinMode(SHT_SFR, OUTPUT); + pinMode(SHT_SBL, OUTPUT); + pinMode(SHT_SBR, OUTPUT); + pinMode(SHT_SLF, OUTPUT); + pinMode(SHT_SLB, OUTPUT); + pinMode(SHT_SRF, OUTPUT); + pinMode(SHT_SRB, OUTPUT); + + Serial.println(F("Shutdown pins inited...")); + + digitalWrite(SHT_SFL, LOW); + digitalWrite(SHT_SFR, LOW); + digitalWrite(SHT_SBL, LOW); + digitalWrite(SHT_SBR, LOW); + digitalWrite(SHT_SLF, LOW); + digitalWrite(SHT_SLB, LOW); + digitalWrite(SHT_SRF, LOW); + digitalWrite(SHT_SRB, LOW); + + + setID(); + delay(50); + + // FreeRTOS tasks +if (WiFi.status() == WL_CONNECTED) { + xTaskCreate(otaTask, "OTA", 4096, NULL, 1, NULL); + xTaskCreate(telnetTask, "Telnet", 4096, NULL, 1, NULL); + } + +} + +void loop() { + + + read_sensors(); + sendSensorData(); + delay(100); + + +} + +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 Sensors"); + } + 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 setID() { + // all reset + digitalWrite(SHT_SFL, LOW); + digitalWrite(SHT_SFR, LOW); + digitalWrite(SHT_SBL, LOW); + digitalWrite(SHT_SBR, LOW); + digitalWrite(SHT_SLF, LOW); + digitalWrite(SHT_SLB, LOW); + digitalWrite(SHT_SRF, LOW); + digitalWrite(SHT_SRB, LOW); + delay(10); + // all unreset + digitalWrite(SHT_SFL, HIGH); + digitalWrite(SHT_SFR, HIGH); + digitalWrite(SHT_SBL, HIGH); + digitalWrite(SHT_SBR, HIGH); + digitalWrite(SHT_SLF, HIGH); + digitalWrite(SHT_SLB, HIGH); + digitalWrite(SHT_SRF, HIGH); + digitalWrite(SHT_SRB, HIGH); + delay(10); + + // activating SFL while keeping all others shutdown + digitalWrite(SHT_SFL, HIGH); + digitalWrite(SHT_SFR, LOW); + digitalWrite(SHT_SBL, LOW); + digitalWrite(SHT_SBR, LOW); + digitalWrite(SHT_SLF, LOW); + digitalWrite(SHT_SLB, LOW); + digitalWrite(SHT_SRF, LOW); + digitalWrite(SHT_SRB, LOW); + + // initing SFL + if(!SFL.begin(SFL_ADDRESS)) { + logPrintln("Failed to boot SFL"); + } else { + logPrintln("SFL OK"); + SFL_OK = true; + } delay(10); + + // activating SFR + digitalWrite(SHT_SFR, HIGH); + delay(10); + //initing SFR + if(!SFR.begin(SFR_ADDRESS)) { + logPrintln("Failed to boot SFR"); + } else { + logPrintln("SFR OK"); + SFR_OK = true; + } + + // activating SBL + digitalWrite(SHT_SBL, HIGH); + delay(10); + //initing SBL + if(!SBL.begin(SBL_ADDRESS)) { + logPrintln("Failed to boot SBL"); + } else { + logPrintln("SBL OK"); + SBL_OK = true; + } + + // activating SBR + digitalWrite(SHT_SBR, HIGH); + delay(10); + //initing SBR + if(!SBR.begin(SBR_ADDRESS)) { + logPrintln("Failed to boot SBR"); + } else { + logPrintln("SBR OK"); + SBR_OK = true; + } + + // activating SLF + digitalWrite(SHT_SLF, HIGH); + delay(10); + //initing SLF + if(!SLF.begin(SLF_ADDRESS)) { + logPrintln("Failed to boot SLF"); + } else { + logPrintln("SLF OK"); + SLF_OK = true; + } + + // activating SLB + digitalWrite(SHT_SLB, HIGH); + delay(10); + //initing SLB +if(!SLB.begin(SLB_ADDRESS)) { + logPrintln("Failed to boot SLB"); + } else { + logPrintln("SLB OK"); + SLB_OK = true; + } + + // activating SRF + digitalWrite(SHT_SRF, HIGH); + delay(10); + //initing SRF + if(!SRF.begin(SRF_ADDRESS)) { + logPrintln("Failed to boot SRF"); + } else { + logPrintln("SRF OK"); + SRF_OK = true; + } + + // activating SRB + digitalWrite(SHT_SRB, HIGH); + delay(10); + //initing SRB +if(!SRB.begin(SRB_ADDRESS)) { + logPrintln("Failed to boot SRB"); + } else { + logPrintln("SRB OK"); + SRB_OK = true; + } +} + +void read_sensors() { + + if(SFL_OK) {SFL.rangingTest(&m_FL, false);} + if(SFR_OK) {SFR.rangingTest(&m_FR, false);} + if(SBL_OK) {SBL.rangingTest(&m_BL, false);} + if(SBR_OK) {SBR.rangingTest(&m_BR, false);} + if(SLF_OK) {SLF.rangingTest(&m_LF, false);} + if(SLB_OK) {SLB.rangingTest(&m_LB, false);} + if(SRF_OK) {SRF.rangingTest(&m_RF, false);} + if(SRB_OK) {SRB.rangingTest(&m_RB, false);} + + + // Print Front left sensor reading + logPrint("Front left: "); + if(!SFL_OK) { + logPrint("NC"); + } else if(m_FL.RangeStatus != 4) { + logPrint(m_FL.RangeMilliMeter); + } else { + logPrint("Out of range"); + } + + logPrint(" "); + + // Print Front right sensor reading + logPrint("Front right: "); + if(!SFR_OK) { + logPrint("NC"); + } else if(m_FR.RangeStatus != 4) { + logPrint(m_FR.RangeMilliMeter); + } else { + logPrint("Out of range"); + } + + logPrint(" "); + + // Print Back left sensor reading + logPrint("Back left: "); + if(!SBL_OK) { + logPrint("NC"); + } else if(m_BL.RangeStatus != 4) { + logPrint(m_BL.RangeMilliMeter); + } else { + logPrint("Out of range"); + } + + + logPrint(" "); + + // Print Back right sensor reading + logPrint("Back right: "); + if(!SBR_OK) { + logPrint("NC"); + } else if(m_BR.RangeStatus != 4) { + logPrint(m_BR.RangeMilliMeter); + } else { + logPrint("Out of range"); + } + + + + + + + logPrint(" "); + + // Print Left front sensor reading + logPrint("Left front: "); + if(!SLF_OK) { + logPrint("NC"); + } else if(m_LF.RangeStatus != 4) { + logPrint(m_LF.RangeMilliMeter); + } else { + logPrint("Out of range"); + } + + logPrint(" "); + + // Print Left back sensor reading + logPrint("Left back: "); + if(!SLB_OK) { + logPrint("NC"); + } else if(m_LB.RangeStatus != 4) { + logPrint(m_LB.RangeMilliMeter); + } else { + logPrint("Out of range"); + } + + logPrint(" "); + + // Print Right front sensor reading + logPrint("Right front: "); + if(!SRF_OK) { + logPrint("NC"); + } else if(m_RF.RangeStatus != 4) { + logPrint(m_RF.RangeMilliMeter); + } else { + logPrint("Out of range"); + } + + logPrint(" "); + + // Print back front sensor reading + logPrint("Right back: "); + if(!SRB_OK) { + logPrint("NC"); + } else if(m_RB.RangeStatus != 4) { + logPrint(m_RB.RangeMilliMeter); + } else { + logPrint("Out of range"); + } + + logPrintln(""); +} + +void sendSensorData() { + // Format: FL,FR,BL,BR,LF,LB,RF,RB\n + Serial2.print(SFL_OK ? m_FL.RangeMilliMeter : -1); Serial2.print(","); + Serial2.print(SFR_OK ? m_FR.RangeMilliMeter : -1); Serial2.print(","); + Serial2.print(SBL_OK ? m_BL.RangeMilliMeter : -1); Serial2.print(","); + Serial2.print(SBR_OK ? m_BR.RangeMilliMeter : -1); Serial2.print(","); + Serial2.print(SLF_OK ? m_LF.RangeMilliMeter : -1); Serial2.print(","); + Serial2.print(SLB_OK ? m_LB.RangeMilliMeter : -1); Serial2.print(","); + Serial2.print(SRF_OK ? m_RF.RangeMilliMeter : -1); Serial2.print(","); + Serial2.println(SRB_OK ? m_RB.RangeMilliMeter : -1); + } +