Electropepper/Compete_car
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Working on Phase 1

Browse Source
This commit is contained in:
Ricardo
2026-02-01 16:51:38 +00:00
parent 470d352ec0
commit 311c462036
1 changed files with 88 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

134
Firmware/micro_motors/micro_motors.ino
View File

@@ -49,7 +49,9 @@ const char* password = "hi123456";
int trimFL = 0; int trimFL = 0;
int trimFR = 0; int trimFR = 0;
int trimBL = 0; int trimBL = 0;
int trimBR = 0; int trimBR = 0;
bool ph1 = false; // Phase 1 complete flag
// Telnet server to watch serial // Telnet server to watch serial
@@ -57,14 +59,14 @@ WiFiServer telnetServer(23);
WiFiClient telnetClient; WiFiClient telnetClient;
// Function declaritons // Function declaritons
void otaTask(void *parameter); // Get micro ready for OTA void otaTask(void *parameter); // Get micro ready for OTA
void logPrint(const char* msg); // Print function for serial and telnet, strings 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(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(const char* msg); // Print line function for serial and telnet
void logPrintln(int val); // Print line function for serial and telnet, integers 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 logPrintln(float val); // Print line function for serial and telnet, floats
void telnetTask(void *parameter); // Configure telnet serial 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 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 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 motorBR(int speed); // Back right motor, speed from -255 to 255, positive goes forward, negative goes backward
@@ -155,28 +157,62 @@ if (WiFi.status() == WL_CONNECTED) {
if (WiFi.status() == WL_CONNECTED) { if (WiFi.status() == WL_CONNECTED) {
xTaskCreate(otaTask, "OTA", 4096, NULL, 1, NULL); xTaskCreate(otaTask, "OTA", 4096, NULL, 1, NULL);
xTaskCreate(telnetTask, "Telnet", 4096, NULL, 1, NULL); xTaskCreate(telnetTask, "Telnet", 4096, NULL, 1, NULL);
} }
} }
void loop() { void loop() {
// ph1 = true;
receiveSensorData(); if (!ph1) {
while ((sens_FL + sens_FR) / 2 > 174) { phaseOne();
motorsGo(180); ph1 = true;
receiveSensorData(); }
}
motorsStop();
delay(1000);
Go90right();
// motorsTest(); // motorsTest();
// sensorsTest(); // 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) { void otaTask(void *parameter) {
for (;;) { for (;;) {
@@ -196,7 +232,7 @@ void telnetTask(void *parameter) {
} }
vTaskDelay(100 / portTICK_PERIOD_MS); vTaskDelay(100 / portTICK_PERIOD_MS);
} }
} }
void logPrint(const char* msg) { void logPrint(const char* msg) {
Serial.print(msg); Serial.print(msg);
@@ -311,30 +347,36 @@ void motorBR(int speed) {
motorBL(0); motorBL(0);
} }
void receiveSensorData() { void receiveSensorData() {
if (Serial2.available()) { String data;
String data = Serial2.readStringUntil('\n');
int values[8]; // Drain buffer - keep only the latest complete line
int idx = 0; while (Serial2.available()) {
int start = 0; data = Serial2.readStringUntil('\n');
}
for (int i = 0; i <= data.length() && idx < 8; i++) {
if (i == data.length() || data[i] == ',') { if (data.length() == 0) return;
values[idx++] = data.substring(start, i).toInt();
start = i + 1; int values[8];
} int idx = 0;
} int start = 0;
if (idx == 8) { for (int i = 0; i <= data.length() && idx < 8; i++) {
sens_FL = values[0]; if (i == data.length() || data[i] == ',') {
sens_FR = values[1]; values[idx++] = data.substring(start, i).toInt();
sens_BL = values[2]; start = i + 1;
sens_BR = values[3]; }
sens_LF = values[4]; }
sens_LB = values[5];
sens_RF = values[6]; if (idx == 8) {
sens_RB = values[7]; 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];
} }
} }
@@ -461,7 +503,7 @@ void Go90right() {
motorFR(-150); motorFR(-150);
motorBL(150); motorBL(150);
motorBR(-150); motorBR(-150);
delay(1735); delay(1150);
motorsStop(); motorsStop();
} }
@@ -470,7 +512,7 @@ void Go90left() {
motorFR(150); motorFR(150);
motorBL(-150); motorBL(-150);
motorBR(150); motorBR(150);
delay(1735); delay(1150);
motorsStop(); motorsStop();
} }