Show page

Differences

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

--- pm:prj2025:cmoarcas:nichita_adrian.bunu [2025/05/13 23:55]
nichita_adrian.bunu
+++ pm:prj2025:cmoarcas:nichita_adrian.bunu [2025/05/30 10:00] (current)
nichita_adrian.bunu
@@ Line 84: / Line 84: @@
 ===== Software Design =====
+Mediu de dezvoltare:
+  * PlatformIO (bazat pe VS Code), cu suport pentru Arduino UNO.
-<note tip>
+Biblioteci utilizate:
-Descrierea codului aplicaţiei (firmware):
-  * mediu de dezvoltare (if any) (e.g. AVR Studio, CodeVisionAVR)
-  * librării şi surse 3rd-party (e.g. Procyon AVRlib)
-  * algoritmi şi structuri pe care plănuiţi să le implementaţi
-  * (etapa 3) surse şi funcţii implementate
-</note>
-===== Rezultate Obţinute =====
+  * I2Cdevlib – pentru comunicarea cu senzorul MPU6050 (accelerometru + giroscop).
-<note tip>
+  * NewPing – pentru măsurarea distanței cu senzorul ultrasonic HC-SR04.
-Care au fost rezultatele obţinute în urma realizării proiectului vostru.
-</note>
-===== Concluzii =====
+Biblioteci standard:
+  * Wire.h, math.h.
-===== Download =====
+Algoritmi și funcționalitate:
-<note warning>
+  * Controlul echilibrului se realizează cu un algoritm PID executat într-un ISR hardware (Timer1, 5ms).
-O arhivă (sau mai multe dacă este cazul) cu fişierele obţinute în urma realizării proiectului: surse, scheme, etc. Un fişier README, un ChangeLog, un script de compilare şi copiere automată pe uC crează întotdeauna o impresie bună ;-).
-Fişierele se încarcă pe wiki folosind facilitatea **Add Images or other files**. Namespace-ul în care se încarcă fişierele este de tipul **:pm:prj20??:c?** sau **:pm:prj20??:c?:nume_student** (dacă este cazul). **Exemplu:** Dumitru Alin, 331CC -> **:pm:prj2009:cc:dumitru_alin**.
+  * Unghiul de înclinare este calculat cu un filtru complementar din datele de accelerometru și giroscop.
-</note>
-===== Jurnal =====
+  * Motoarele sunt controlate prin PWM bidirecțional, iar sistemul reacționează la obstacole apropiate (<20 cm) prin inversarea direcției.
+  * Un LED (pin 13) indică funcționarea periodică (1 Hz).
 <note tip>
-Puteți avea și o secțiune de jurnal în care să poată urmări asistentul de proiect progresul proiectului.
-</note>
-===== Bibliografie/Resurse =====
+#include <Wire.h>
+#include <I2Cdev.h>
+#include <MPU6050.h>
+#include <math.h>
+#include <NewPing.h>
+#define leftMotorPWMPin 6
+#define leftMotorDirPin 7
+#define rightMotorPWMPin 5
+#define rightMotorDirPin 4
+#define TRIGGER_PIN 9
+#define ECHO_PIN 8
+#define MAX_DISTANCE 75
+#define Kp 40
+#define Kd 0.05
+#define Ki 40
+#define sampleTime 0.005
+#define targetAngle -2.5
+MPU6050 mpu;
+NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
+int16_t accY, accZ, gyroX;
+volatile int motorPower, gyroRate;
+volatile float accAngle, gyroAngle, currentAngle, prevAngle = 0;
+volatile float error, prevError = 0, errorSum = 0;
+volatile byte count = 0;
+int distanceCm;
+void setMotors(int leftMotorSpeed, int rightMotorSpeed) {
+    if (leftMotorSpeed >= 0) {
+        analogWrite(leftMotorPWMPin, leftMotorSpeed);
+        digitalWrite(leftMotorDirPin, LOW);
+    } else {
+        analogWrite(leftMotorPWMPin, 255 + leftMotorSpeed);
+        digitalWrite(leftMotorDirPin, HIGH);
+    }
+    if (rightMotorSpeed >= 0) {
+        analogWrite(rightMotorPWMPin, rightMotorSpeed);
+        digitalWrite(rightMotorDirPin, LOW);
+    } else {
+        analogWrite(rightMotorPWMPin, 255 + rightMotorSpeed);
+        digitalWrite(rightMotorDirPin, HIGH);
+    }
+}
+void init_PID() {
+    cli(); // disable global interrupts
+    TCCR1A = 0;
+    TCCR1B = 0;
+    OCR1A = 9999;
+    TCCR1B |= (1 << WGM12); // CTC mode
+    TCCR1B |= (1 << CS11);  // prescaler 8
+    TIMSK1 |= (1 << OCIE1A); // enable timer interrupt
+    sei(); // enable global interrupts
+}
+void setup() {
+    pinMode(leftMotorPWMPin, OUTPUT);
+    pinMode(leftMotorDirPin, OUTPUT);
+    pinMode(rightMotorPWMPin, OUTPUT);
+    pinMode(rightMotorDirPin, OUTPUT);
+    pinMode(13, OUTPUT);
+    Wire.begin();
+    mpu.initialize();
+    mpu.setYAccelOffset(1593);
+    mpu.setZAccelOffset(963);
+    mpu.setXGyroOffset(40);
+    init_PID();
+}
+void loop() {
+    accY = mpu.getAccelerationY();
+    accZ = mpu.getAccelerationZ();
+    gyroX = mpu.getRotationX();
+    motorPower = constrain(motorPower, -255, 255);
+    setMotors(motorPower, motorPower);
+    if ((count % 20) == 0) {
+        distanceCm = sonar.ping_cm();
+    }
+    if ((distanceCm < 20) && (distanceCm != 0)) {
+        setMotors(-motorPower, motorPower);
+    }
+}
+ISR(TIMER1_COMPA_vect) {
+    accAngle = atan2(accY, accZ) * RAD_TO_DEG;
+    gyroRate = map(gyroX, -32768, 32767, -250, 250);
+    gyroAngle = (float)gyroRate * sampleTime;
+    currentAngle = 0.9934 * (prevAngle + gyroAngle) + 0.0066 * accAngle;
+    error = currentAngle - targetAngle;
+    errorSum += error;
+    errorSum = constrain(errorSum, -300, 300);
-<note>
+    motorPower = Kp * error + Ki * errorSum * sampleTime - Kd * (currentAngle - prevAngle) / sampleTime;
-Listă cu documente, datasheet-uri, resurse Internet folosite, eventual grupate pe **Resurse Software** şi **Resurse Hardware**.
+    prevAngle = currentAngle;
-</note>
-<html><a class="media mediafile mf_pdf" href="?do=export_pdf">Export to PDF</a></html>
+    count++;
+    if (count == 200) {
+        count = 0;
+        digitalWrite(13, !digitalRead(13));
+    }
+}