diff --git a/Firmware/micro_motors/micro_motors.ino b/Firmware/micro_motors/micro_motors.ino index 219e366..56f5e07 100644 --- a/Firmware/micro_motors/micro_motors.ino +++ b/Firmware/micro_motors/micro_motors.ino @@ -1,54 +1,54 @@ -#include +#include #include -const char* ssid = "CIA"; -const char* password = "hi123456"; +const char* ssid = "CIA"; +const char* password = "hi123456"; -#define motorAttach ledcAttach // Get more appropriate names +#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 FR (Front Right) + #define FR_IN1 27 + #define FR_IN2 14 + #define FR_PWM 0 -// 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 + // 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 + // 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; + // 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 trimFL = 0; + int trimFR = 0; + int trimBL = 0; int trimBR = 0; #define BUFFER_SIZE 7 int buffer_FL[BUFFER_SIZE]; @@ -62,28 +62,28 @@ const char* password = "hi123456"; int bufferIndex = 0; bool bufferFilled = false; - bool ph1 = false; // Phase 1 complete flag + 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; +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 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 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(); @@ -102,103 +102,103 @@ 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"); - } + WiFi.mode(WIFI_STA); + WiFi.begin(ssid, password); - -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(); + int wifiAttempts = 0; + while (WiFi.waitForConnectResult() != WL_CONNECTED && wifiAttempts < 10) { + Serial.println("WiFi connecting..."); + wifiAttempts++; + delay(500); } - Serial2.begin(115200, SERIAL_8N1, SERIAL_RX, -1); + if (WiFi.status() == WL_CONNECTED) { + Serial.print("IP Address: "); + Serial.println(WiFi.localIP()); + } else { + Serial.println("WiFi not available - running without network"); + } - // 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); + +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(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); + // 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); + 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); - } - + 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; + ph3 = true; if (!ph1) { phaseOne(); ph1 = true; } - + if (!ph2) { phaseTwo(); ph2 = true; } - + if (!ph3) { phaseThree(); ph3 = true; } // motorsTest(); - sensorsTest(); +// sensorsTest(); } @@ -235,10 +235,10 @@ void phaseOne() { motorsGo(80); delay(1250); // Go into the pit 1800 miliseconds, motorsStop(); - delay(3000); //how long to pick balls? dunno + delay(3000); //how long to pick balls? dunno motorsGo(-150); delay(1200); // Go out of the pit 1800 miliseconds - motorsStop(); + motorsStop(); delay(500); slideLeft(); delay(1450); @@ -256,14 +256,14 @@ void phaseTwo() { receiveSensorData(); - while (!p2_stop) { +// while (!p2_stop) { while ((sens_FL + sens_FR) / 2 > 8000) { - motorsGo(-255); + motorsGo(-230); 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); + while ((sens_FL + sens_FR) / 2 < 800) { // Used to be 680 + motorsGo(-250); delay(5); // Wait for new sensor data to arrive receiveSensorData(); // Get fresh reading each iteration } @@ -272,9 +272,9 @@ void phaseTwo() { delay(5); // Wait for new sensor data to arrive receiveSensorData(); // Get fresh reading each iteration } - p2_stop = true; - receiveSensorData(); - } +// p2_stop = true; +// receiveSensorData(); +// } motorsStop(); @@ -286,147 +286,147 @@ void phaseThree() { } -void otaTask(void *parameter) { - for (;;) { - ArduinoOTA.handle(); - vTaskDelay(10 / portTICK_PERIOD_MS); - } +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 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 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 logPrint(const char* msg) { + Serial.print(msg); + if (telnetClient && telnetClient.connected()) { + telnetClient.print(msg); + } + } -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 logPrint(int val) { + Serial.print(val); + if (telnetClient && telnetClient.connected()) { + telnetClient.print(val); + } + } - void motorsStop() { - motorFR(0); - motorFL(0); - motorBR(0); - motorBL(0); + 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; } @@ -460,155 +460,155 @@ void motorBR(int speed) { 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); - + 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..."); + 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); + delay(2000); - logPrintln("Front forward..."); - motorFR(250); - motorFL(250); - delay(2000); + logPrintln("Right stop..."); + motorBR(0); + motorFR(0); + delay(1000); - logPrintln("Front stop..."); - motorsStop(); - delay(1000); - - logPrintln("Back forward..."); - motorBR(80); - motorBL(80); - delay(2000); - - logPrintln("Back stop..."); - motorsStop(); - delay(1000); - - logPrintln("Back forward..."); + logPrintln("Right forward..."); motorBR(250); - motorBL(250); - delay(2000); + motorFR(250); + delay(2000); - logPrintln("Back stop..."); + logPrintln("Right stop..."); + motorBR(0); + motorFR(0); + delay(1000); + + logPrintln("Left forward..."); + motorBL(80); + motorFL(80); + delay(2000); + + logPrintln("Left stop..."); motorsStop(); - delay(1000); + 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); + motorFR(speed); + motorFL(speed); + motorBR(speed); + motorBL(speed); } void Go45right() { - motorFL(150); - motorFR(-150); - motorBL(150); + motorFL(150); + motorFR(-150); + motorBL(150); motorBR(-150); delay(875); motorsStop(); } void Go45left() { - motorFL(-150); - motorFR(150); - motorBL(-150); + motorFL(-150); + motorFR(150); + motorBL(-150); motorBR(150); delay(875); motorsStop(); } void Go90right() { - motorFL(150); - motorFR(-150); - motorBL(150); + motorFL(150); + motorFR(-150); + motorBL(150); motorBR(-150); delay(1250); motorsStop(); } void Go90left() { - motorFL(-150); - motorFR(150); - motorBL(-150); + motorFL(-150); + motorFR(150); + motorBL(-150); motorBR(150); delay(1250); motorsStop(); } void slideRight() { - motorFL(-150); - motorFR(150); - motorBL(150); + motorFL(-150); + motorFR(150); + motorBL(150); motorBR(-150); } void slideLeft() { - motorFL(150); - motorFR(-150); - motorBL(-150); + motorFL(150); + motorFR(-150); + motorBL(-150); motorBR(150); }