Show page

Differences

This shows you the differences between two versions of the page.

--- pm:prj2025:vradulescu:mihnea.stamatie [2025/05/23 17:00]
mihnea.stamatie [Bibliografie/Resurse]
+++ pm:prj2025:vradulescu:mihnea.stamatie [2025/05/23 17:02] (current)
mihnea.stamatie [Concluzii]
@@ Line 314: / Line 314: @@
 Rezultatul este un robot stabil și predictibil, capabil să urmărească trasee complexe cu precizie ridicată.
+<code cpp>
+#include <Arduino.h>
+double error;
+double lastError;
+double errorDiff;
+double errorInt;
+#define KP 17.00
+#define KD 600.00
+#define KI 0.00
+#define baseSpeed 40.00
+#define nearLineModifier 0.50
+struct motor {
+    int motorControllerForwardPIN{};
+    int motorControllerBackwardPIN{};
+    int motorControllerSpeedPIN{};
+    int internalPower{};
+    int mappedPower{};
+    bool internalOrientation = false;
+    void setOrientation(const bool orientation) {
+        internalOrientation = orientation;
+    }
+    void setPins(const int forwardPIN, const int backwardPIN, const int speedPWMPIN) {
+        motorControllerForwardPIN = forwardPIN;
+        motorControllerBackwardPIN = backwardPIN;
+        motorControllerSpeedPIN = speedPWMPIN;
+        pinMode(motorControllerForwardPIN, OUTPUT);
+        pinMode(motorControllerBackwardPIN, OUTPUT);
+        pinMode(motorControllerSpeedPIN, OUTPUT);
+    }
+    void setPower(int power) {
+        power = constrain(power, -100, 100);
+        mappedPower = power;
+        if (internalOrientation)
+            power *= -1;
+        internalPower = static_cast<int>(map(abs(power), 0, 100, 0, 255));
+        if (power < 0) {
+            digitalWrite(motorControllerForwardPIN, LOW);
+            digitalWrite(motorControllerBackwardPIN, HIGH);
+        } else {
+            digitalWrite(motorControllerBackwardPIN, LOW);
+            digitalWrite(motorControllerForwardPIN, HIGH);
+        }
+        analogWrite(motorControllerSpeedPIN, internalPower);
+    }
+};
+motor Right;
+motor Left;
+struct lineSensor {
+    bool leftValue = false;
+    bool leftCenterValue = false;
+    bool centerValue = false;
+    bool rightCenterValue = false;
+    bool rightValue = false;
+    double globalLineValue = 0;
+    String lineOutput;
+    int lastGlobalLineValue = 0;
+    int leftSensorPIN = 0;
+    int leftCenterSensorPIN = 0;
+    int centerSensorPIN = 0;
+    int rightCenterSensorPIN = 0;
+    int rightSensorPIN = 0;
+    void setPins(const int left, const int leftCenter, const int center, const int rightCenter, const int right) {
+        leftSensorPIN = left;
+        leftCenterSensorPIN = leftCenter;
+        centerSensorPIN = center;
+        rightCenterSensorPIN = rightCenter;
+        rightSensorPIN = right;
+        pinMode(leftSensorPIN, INPUT);
+        pinMode(leftCenterSensorPIN, INPUT);
+        pinMode(centerSensorPIN, INPUT);
+        pinMode(rightCenterSensorPIN, INPUT);
+        pinMode(rightSensorPIN, INPUT);
+    }
+    void LineValueAnalyzer() {
+        if (!noDetection())
+            lastGlobalLineValue = static_cast<int>(globalLineValue);
+        leftValue = !digitalRead(leftSensorPIN);
+        leftCenterValue = !digitalRead(leftCenterSensorPIN);
+        centerValue = !digitalRead(centerSensorPIN);
+        rightCenterValue = !digitalRead(rightCenterSensorPIN);
+        rightValue = !digitalRead(rightSensorPIN);
+        lineOutput = leftValue + static_cast<String>(leftCenterValue) + static_cast<String>(centerValue) + static_cast<
+                         String>(rightCenterValue) + static_cast<String>(rightValue);
+        if (lineOutput == "10000") {
+            globalLineValue = -6; // -5
+        } else if (lineOutput == "11000") {
+            globalLineValue = -5; // -3
+        } else if (lineOutput == "01000") {
+            globalLineValue = -2 * nearLineModifier;
+        } else if (lineOutput == "01100") {
+            globalLineValue = -1 * nearLineModifier;
+        } else if (lineOutput == "00100") {
+            globalLineValue = 0;
+        } else if (lineOutput == "00110") {
+            globalLineValue = 1 * nearLineModifier;
+        } else if (lineOutput == "00010") {
+            globalLineValue = 2 * nearLineModifier;
+        } else if (lineOutput == "00011") {
+            globalLineValue = 5; // 3
+        } else if (lineOutput == "00001") {
+            globalLineValue = 6; // 5
+        } else if (lineOutput == "11100" || lineOutput == "11110" || lineOutput == "10100" || lineOutput == "10010") {
+            globalLineValue = -6;
+        } else if (lineOutput == "00111" || lineOutput == "01111" || lineOutput == "00101" || lineOutput == "01001") {
+            globalLineValue = 6;
+        } else if (lineOutput == "11111" || lineOutput == "01110" || lineOutput == "01010") {
+            globalLineValue = lastGlobalLineValue;
+        }
+    }
+    bool noDetection() {
+        leftValue = digitalRead(leftSensorPIN);
+        leftCenterValue = digitalRead(leftCenterSensorPIN);
+        centerValue = digitalRead(centerSensorPIN);
+        rightCenterValue = digitalRead(rightCenterSensorPIN);
+        rightValue = digitalRead(rightSensorPIN);
+        if (leftValue == 1 && leftCenterValue == 1 && centerValue == 1 && rightCenterValue == 1 && rightValue == 1)
+            return true;
+        return false;
+    }
+    int returnLineValue() {
+        if (noDetection()) {
+            Serial.println(lastGlobalLineValue);
+            return lastGlobalLineValue;
+        }
+        Serial.println(globalLineValue);
+        return static_cast<int>(globalLineValue);
+    }
+    void SerialLineAnalyzer(const String &command) {
+        LineValueAnalyzer();
+        if (command == "every sensor") {
+            Serial.print(leftValue);
+            Serial.print(" ");
+            Serial.print(leftCenterValue);
+            Serial.print(" ");
+            Serial.print(centerValue);
+            Serial.print(" ");
+            Serial.print(rightCenterValue);
+            Serial.print(" ");
+            Serial.print(rightValue);
+            Serial.print("\n");
+        }
+        if (command == "all") {
+            Serial.print(leftValue);
+            Serial.print(" ");
+            Serial.print(leftCenterValue);
+            Serial.print(" ");
+            Serial.print(centerValue);
+            Serial.print(" ");
+            Serial.print(rightCenterValue);
+            Serial.print(" ");
+            Serial.print(rightValue);
+            Serial.print("       ");
+            Serial.print(globalLineValue);
+            Serial.print(" ");
+        }
+        if (command == "value") {
+            Serial.print(globalLineValue);
+            Serial.print("\n");
+        }
+    }
+};
+lineSensor lineSensorModule;
+void setup() {
+    Serial.begin(9600);
+    Right.setOrientation(false);
+    Left.setOrientation(true);
+    Right.setPins(4, 5, 3);
+    Left.setPins(7, 8, 6);
+    lineSensorModule.setPins(9, 10, 11, 12, 13);
+}
+double output;
+double PID(int input) {
+    error = error * 0.4 + input * 0.6;
+    errorDiff = error - lastError;
+    errorInt = constrain(error + errorInt, -20, 20);
+    output = KP * error + KD * errorDiff + KI * errorInt;
+    lastError = error;
+    return output;
+}
+void loop() {
+    lineSensorModule.LineValueAnalyzer();
+    Left.setPower(baseSpeed - PID(lineSensorModule.returnLineValue()));
+    Right.setPower(baseSpeed + PID(lineSensorModule.returnLineValue()));
+}
+</code>
 ===== Download =====