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