From e3bd343fb3acf10191127e10a35c8cc177ee739f Mon Sep 17 00:00:00 2001 From: lucasrguerra Date: Wed, 1 May 2024 01:21:34 -0300 Subject: [PATCH] code-review --- Perseguidor_VESPA.ino | 369 ++++++++++++++++++++++---------------- Perseguidor_VESPA/txt.txt | 1 - 2 files changed, 217 insertions(+), 153 deletions(-) delete mode 100644 Perseguidor_VESPA/txt.txt diff --git a/Perseguidor_VESPA.ino b/Perseguidor_VESPA.ino index 984a12d..e27b0b0 100644 --- a/Perseguidor_VESPA.ino +++ b/Perseguidor_VESPA.ino @@ -6,148 +6,123 @@ * from RoboCore and the Pololu's QTR-8RC sensor ****************************************************************/ -#define DEBUG -#define BT_NAME "Motoneta" -// Names: Mutuca | Motoneta | Van Dyne +#define DEBUG // Debug mode +#define BT_NAME "Motoneta" // Names: Mutuca | Motoneta | Van Dyne + + #ifdef DEBUG -#include "BluetoothSerial.h" + #include "BluetoothSerial.h" // Bluetooth Serial library + BluetoothSerial SerialBT; // Bluetooth Serial instance -#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) -#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it -#endif -BluetoothSerial SerialBT; // Bluetooth Serial instance + #if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) + #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it + #endif #endif -#include // Library for the Vespa microcontroller -#include // Library for the QTR-8A or the QTR-8RC -VespaMotors motor; // Vespa Motor Object -QTRSensors qtr; // QTR Sensor -// Set button and led pins +#include // Library for the Vespa microcontroller +#include // Library for the QTR-8A or the QTR-8RC + + + +// ----------------- Objects ----------------- +VespaMotors motor; // Vespa Motor Object +QTRSensors qtr; // QTR Sensor +// ------------------------------------------- + + +// ----------------- Constants ----------------- const uint8_t PIN_BUTTON = 35; const uint8_t PIN_LED = 15; const uint8_t PIN_MARKER_SENSOR = 36; +const uint8_t SENSOR_COUNT = 8; +const int16_t MAX_POSITION = (SENSOR_COUNT - 1) * 1000; +const uint8_t PINS_SENSOR[SENSOR_COUNT] = { 21, 19, 5, 16, 22, 23, 18, 17 }; +const bool LINE_BLACK = false; +// --------------------------------------------- -//Setup of the module of sensors -const uint8_t SENSOR_COUNT = 8; // The number of sensors, which should match the length of the pins array -uint16_t sensorValues[SENSOR_COUNT]; // An array in which to store the calibrated sensor readings -// Maximum line position, considering the amount of sensors. -const long MAX_POSITION = (SENSOR_COUNT - 1) * 1000; -//Marker sensor variables -unsigned long startMakerChecker = 39500L; -unsigned long initialTime; +// ----------------- Variables ----------------- +uint16_t sensorValues[SENSOR_COUNT]; +uint32_t startMakerChecker = 39500L; +uint32_t initialTime; +uint8_t marginError = 20; +bool firstRun = true; +// --------------------------------------------- -// Limit value of the margin of error -int marginError = 20; -bool firstRun = true; -//------------------PID Control------------------- -float p = 0, i = 0, d = 0, pid = 0, error = 0, lastError = 0; +// ----------------- PID Control ----------------- +float p = 0; +float i = 0; +float d = 0; +float pid = 0; +float error = 0; +float lastError = 0; float Kp = 0.3; float Ki = 0.0001; float Kd = 3.5; int maxSpeed = 100; -int lSpeed, rSpeed; +int leftSpeed; +int rightSpeed; +// ----------------------------------------------- -const bool LINE_BLACK = false; -//------------------------------------------------- -void setup() { - qtr.setTypeRC(); // For QTR-8RC Sensor pins: - qtr.setSensorPins((const uint8_t[]){ 21, 19, 5, 16, 22, 23, 18, 17 }, SENSOR_COUNT); - pinMode(PIN_BUTTON, INPUT); - pinMode(PIN_MARKER_SENSOR, INPUT); - pinMode(PIN_LED, OUTPUT); +// ----------------- Functions ----------------- +void calibrateSensors(); +int readSensors(); +bool markerChecker(); #ifdef DEBUG - if (firstRun) { - Serial.begin(115200); - delay(100); - - SerialBT.begin(BT_NAME); // Bluetooth device name - firstRun = false; - } - - SerialBT.println("Start BT communication"); + String receiveBluetoothMessage(); + String getElement(String data, int index); + void setupBluetooth(); + void printParametersInBluetooth(); + void printCalibrationInBluetooth(); +#endif +// --------------------------------------------- - String btMessage; - String prefix; - while (prefix != "end" && digitalRead(PIN_BUTTON) == HIGH) { - btMessage = receiveBtMessage(); - prefix = getPrefix(btMessage); - if (prefix == "pid") { - Kp = getNumber(btMessage, 1); - Ki = getNumber(btMessage, 2); - Kd = getNumber(btMessage, 3); - } else if (prefix == "spe") { - maxSpeed = getNumber(btMessage, 1); - } else if (prefix == "tim") { - startMakerChecker = getNumber(btMessage, 1); - } else if (prefix == "err") { - marginError = getNumber(btMessage, 1); - } else if (prefix == "pri") { - printParameters(); - } else if (prefix == "end") { - break; - } else { - SerialBT.println("This command doesn't exists!"); - } - } - - printParameters(); - - SerialBT.println("Start Calibration..."); +void setup() { + qtr.setTypeRC(); + qtr.setSensorPins(PINS_SENSOR, SENSOR_COUNT); - delay(500); -#endif + pinMode(PIN_BUTTON, INPUT); + pinMode(PIN_MARKER_SENSOR, INPUT); + pinMode(PIN_LED, OUTPUT); - // Calibration - digitalWrite(PIN_LED, HIGH); - while (digitalRead(PIN_BUTTON) == HIGH) { // Calibrates until the button is pressed - qtr.calibrate(); - } - digitalWrite(PIN_LED, LOW); + #ifdef DEBUG + if (firstRun) { + Serial.begin(115200); + delay(100); - SerialBT.println("Wating command ..."); - while( prefix != "go"){ - btMessage = receiveBtMessage(); - prefix = getPrefix(btMessage); - } + SerialBT.begin(BT_NAME); + firstRun = false; + } + setupBluetooth(); + #endif -#ifdef DEBUG - // Print the calibration minimum values measured when emitters were on - for (uint8_t i = 0; i < SENSOR_COUNT; i++) { - SerialBT.print(qtr.calibrationOn.minimum[i]); - SerialBT.print(' '); - } - SerialBT.println(); + calibrateSensors(); - // Print the calibration maximum values measured when emitters were on - for (uint8_t i = 0; i < SENSOR_COUNT; i++) { - SerialBT.print(qtr.calibrationOn.maximum[i]); - SerialBT.print(' '); - } - SerialBT.println(); -#endif + #ifdef DEBUG + printCalibrationInBluetooth(); + #endif - delay(2000); // Start loop after 2 seconds + delay(2000); initialTime = millis(); } + + void loop() { - // readSensors() returns the line position between 0 and `MAX_POSITION`. - // error is a re-map from -1000 to 1000 range. error = map(readSensors(), 0, MAX_POSITION, -1000, 1000); // Calculate PID @@ -157,43 +132,59 @@ void loop() { pid = (Kp * p) + (Ki * i) + (Kd * d); lastError = error; - // Control Motors - lSpeed = maxSpeed + pid; - rSpeed = maxSpeed - pid; + // Control the motors + leftSpeed = maxSpeed + pid; + rightSpeed = maxSpeed - pid; + leftSpeed = constrain(leftSpeed, -maxSpeed, maxSpeed); + rightSpeed = constrain(rightSpeed, -maxSpeed, maxSpeed); - lSpeed = constrain(lSpeed, -maxSpeed, maxSpeed); - rSpeed = constrain(rSpeed, -maxSpeed, maxSpeed); - - if (markerChecker()) { // Count the markers and stop the robot when reach a certain number + if (markerChecker()) { // Count the markers and stop the robot when reach a certain number motor.stop(); -#ifdef DEBUG - SerialBT.print(">> Timelapse: "); - SerialBT.print(millis() - initialTime); - SerialBT.println(" seconds"); -#endif + + #ifdef DEBUG + SerialBT.print(">> Timelapse: "); + SerialBT.print(millis() - initialTime); + SerialBT.println(" seconds"); + #endif + setup(); - } else if (error >= -marginError && error <= marginError) { // If the error is within the MARGIN_ERROR, move on + } else if (error >= -marginError && error <= marginError) { motor.turn(maxSpeed, maxSpeed); - } else { // If the error is outside the error range, continue doing PID - motor.turn( rSpeed, lSpeed); + } else { + motor.turn(rightSpeed, leftSpeed); } } -int readSensors() { - if (LINE_BLACK) { - return qtr.readLineBlack(sensorValues); - } else { - return qtr.readLineWhite(sensorValues); + + +void calibrateSensors() { + digitalWrite(PIN_LED, HIGH); + while (digitalRead(PIN_BUTTON) == HIGH) { + qtr.calibrate(QTR_EMITTERS_ON); } + digitalWrite(PIN_LED, LOW); } + + /** - Verifies if there is a end line after a set time + * Read the sensors and return the line position + * + * @return The line position between 0 and `MAX_POSITION`. +*/ +int readSensors() { + if (LINE_BLACK) { return qtr.readLineBlack(sensorValues); } + else { return qtr.readLineWhite(sensorValues); } +} + + - @return `true` if the end line was detected. +/** + * Verifies if there is a end line after a set time + * + * @return `true` if the end line was detected. */ bool markerChecker() { - static int i = 0; if (startMakerChecker < millis() - initialTime) { if (analogRead(PIN_MARKER_SENSOR) < 2000) { return true; @@ -203,15 +194,17 @@ bool markerChecker() { return false; } -#ifdef DEBUG -/** - Returns all stream of data sent over bluetooth until the - button is pressed. - @return `String` with the message sent by the bluetooth device +#ifdef DEBUG // Functions for debug mode + +/** + * Returns all stream of data sent over bluetooth until the + * button is pressed. + * + * @return `String` with the message sent by the bluetooth device */ -String receiveBtMessage() { +String receiveBluetoothMessage() { String message; char incomingChar; @@ -231,22 +224,15 @@ String receiveBtMessage() { return message; } -String getPrefix(String data) { - return getElement(data, 0); -} -double getNumber(String data, int index) { - return atof(getElement(data, index).c_str()); -} /** - Returns a sub-string in the `String` data, in the index - position. - - @param `data` String with the message - @param `index` Position of the element to be returned - @return `String` sub-string in the indicated position. If there is - no value at this position, it returns empty string. + * Returns a sub-string in the `String` data, in the index + * position. + * + * @param `data` String with the message + * @param `index` Position of the element to be returned + * @return `String` sub-string in the indicated position. If there is no value at this position, it returns empty string. */ String getElement(String data, int index) { char separator = ' '; @@ -269,23 +255,102 @@ String getElement(String data, int index) { return data.substring(startIndex, endIndex); } -void printParameters() { - SerialBT.println("Configured parameters:"); - SerialBT.print(">> P: "); + + +/** + * Returns the prefix of the message sent by bluetooth + * + * @param `data` String with the message + * @return `String` with the prefix of the message +*/ +String getPrefix(String data) { + return getElement(data, 0); +} + + + +/** + * Returns the number in the index position of the message sent by bluetooth + * + * @param `data` String with the message + * @param `index` Position of the number to be returned + * @return `double` number in the indicated position +*/ +double getNumber(String data, int index) { + return atof(getElement(data, index).c_str()); +} + + + +void setupBluetooth() { + SerialBT.println("Start BT communication"); + + String btMessage; + String prefix; + + while (prefix != "end" && digitalRead(PIN_BUTTON) == HIGH) { + btMessage = receiveBluetoothMessage(); + prefix = getPrefix(btMessage); + + if (prefix == "pid") { + Kp = getNumber(btMessage, 1); + Ki = getNumber(btMessage, 2); + Kd = getNumber(btMessage, 3); + } + else if (prefix == "spe") { maxSpeed = getNumber(btMessage, 1); } + else if (prefix == "tim") { startMakerChecker = getNumber(btMessage, 1); } + else if (prefix == "err") { marginError = getNumber(btMessage, 1); } + else if (prefix == "pri") { printParametersInBluetooth(); } + else if (prefix == "end") { break; } + else { SerialBT.println("This command doesn't exists!"); } + } + + printParametersInBluetooth(); + delay(500); + SerialBT.println("Start Calibration..."); +} + + + +void printParametersInBluetooth() { + SerialBT.println(f("Configured parameters:")); + SerialBT.print(">> Kp: "); SerialBT.print(Kp, 4); - SerialBT.print(" | I: "); + SerialBT.print(" | Ki: "); SerialBT.print(Ki, 4); SerialBT.print(" | Kd: "); SerialBT.println(Kd, 4); - SerialBT.print(">> Speed: "); - SerialBT.println(maxSpeed); + SerialBT.print(">> Max Speed: "); + SerialBT.print(maxSpeed); SerialBT.print(">> Time delay: "); - SerialBT.println(startMakerChecker); + SerialBT.print(startMakerChecker); - SerialBT.print(">> Margin Error: "); + SerialBT.print(">> Error margin: "); SerialBT.println(marginError); } -#endif + + +void printCalibrationInBluetooth() { + SerialBT.println("Wating command ..."); + while( prefix != "go"){ + btMessage = receiveBtMessage(); + prefix = getPrefix(btMessage); + } + + for (uint8_t i = 0; i < SENSOR_COUNT; i++) { + SerialBT.print(qtr.calibratedMinimumOn[i]); + SerialBT.print(" "); + } + SerialBT.println(); + + for (uint8_t i = 0; i < SENSOR_COUNT; i++) { + SerialBT.print(qtr.calibratedMaximumOn[i]); + SerialBT.print(" "); + } + SerialBT.println(); +} + +#endif // End of functions for debug mode \ No newline at end of file diff --git a/Perseguidor_VESPA/txt.txt b/Perseguidor_VESPA/txt.txt deleted file mode 100644 index 8b13789..0000000 --- a/Perseguidor_VESPA/txt.txt +++ /dev/null @@ -1 +0,0 @@ -