#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); }