Show page

Differences

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

--- pm:prj2025:cmoarcas:nichita_adrian.bunu [2025/05/05 18:05]
nichita_adrian.bunu
+++ pm:prj2025:cmoarcas:nichita_adrian.bunu [2025/05/30 10:00] (current)
nichita_adrian.bunu
@@ Line 1: / Line 1: @@
 ====== Self-Balancing Robot ======
-===== Introducere =====
+===== Introduction =====
-<note tip>
+Self-balancing robot capable of moving while avoiding obstacles. This is a small-sized robot based on the Arduino Pro Mini development board and the MPU6050 accelerometer-gyroscope module.
-Robot autobalansat care se poate deplasa evitând obstacolele. Acesta este un robot de dimensiuni reduse și este bazat pe placa de dezvoltare Arduino Pro Mini și pe modulul accelerometru-giroscop MPU6050.
-Acesta imi va fi de folos in ințelegerea conceptului de autobalansare, pentru un viitor proiect de robot OFF-ROAD. Mă voi folosi de biblioteca PID in acest scop.
+It will help me understand the concept of self-balancing for a future OFF-ROAD robot project. I will use the PID library for this purpose.
-</note>
+===== General Description =====
-===== Descriere generală =====
-<note tip>
+{{pm:prj2025:cmoarcas:bloc.jpg?800| Block diagram of the system}}
-O schemă bloc cu toate modulele proiectului vostru, atât software cât şi hardware însoţită de o descriere a acestora precum şi a modului în care interacţionează.
-Exemplu de schemă bloc: http://www.robs-projects.com/mp3proj/newplayer.html
+MPU9250 (Accelerometer + Gyroscope + Magnetometer Module)
+Connected to the Arduino via I²C:
+  *SCL → A5 (SCL)
+  *SDA → A4 (SDA)
+  *Provides motion, tilt, and orientation data for balance control.
+Arduino Pro Mini
+  *Central microcontroller managing sensor input and motor control.
+  *Receives data from MPU9250 and controls the motor driver.
+  *Pins 2, 3, 4, 5, 6, 7, 8, and 9 are used for communication and control.
+TB6612FNG (Motor Driver Module)
+  *Receives control signals from Arduino to drive the left and right motors.
+  *Outputs:
+  *BOUT1, BOUT2 → Left Motor
+  *AOUT1, AOUT2 → Right Motor
+HC-SR04 (Ultrasonic Distance Sensor)
+  *Connected to Arduino for obstacle detection.
+  *Uses TRIG and ECHO pins for distance measurements.
+  *Helps in obstacle avoidance while balancing.
+V Regulators (x2)
+  *Regulate power from the battery to provide a stable 5V supply.
+  *Connected to battery terminals via red (+) and black (–) lines.
+Battery (NCR18650)
+  *Provides the primary power source for all components via terminals.
+  *Positive and negative rails are color-coded: red for +, black for –.
+<note tip> Functional Flow
+The MPU9250 senses tilt, acceleration, and orientation, sending data to the Arduino, which processes it using a PID algorithm.
+Based on the tilt angle, Arduino sends commands to the TB6612FNG motor driver to adjust the rotation of the left and right motors to balance the robot.
+The HC-SR04 sensor detects nearby obstacles and is used to alter motor behavior to avoid collisions.
 </note>
 ===== Hardware Design =====
-<note tip>
+{{pm:prj2025:cmoarcas:circuit_modificat_nichita.jpg?800| Wiring diagram}}
-Aici puneţi tot ce ţine de hardware design:
-  * listă de piese
+{{pm:prj2025:cmoarcas:circuit_image_nichita.png?800| Realistic wiring diagram}}
-  * scheme electrice (se pot lua şi de pe Internet şi din datasheet-uri, e.g. http://www.captain.at/electronic-atmega16-mmc-schematic.png)
-  * diagrame de semnal
+^ Name ^ Description ^
-  * rezultatele simulării
+| Arduino Pro Mini | Controller for modules and central processing unit |
-</note>
+| MPU9250 Module (accelerometer + gyroscope + magnetometer) | Detects tilt, acceleration, and orientation for balance control |
+| TB6612FNG Motor Driver | Drives left and right motors based on Arduino commands |
+| HC-SR04 Distance Sensor | Measures distance to obstacles for collision avoidance |
+| 5V Regulators (2x) | Stabilize voltage supply for Arduino, sensors, and motors |
+| NCR18650 Battery | Main power source for the entire system |
+| DC Motors | Provide motion to balance and move the robot |
+| Jumper Wires | Electrical connections between components |
+| XH2.54 Connectors | Secure and detachable module connections |
 ===== 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>
+  * Motoarele sunt controlate prin PWM bidirecțional, iar sistemul reacționează la obstacole apropiate (<20 cm) prin inversarea direcției.
-===== Jurnal =====
+  * 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>
-<note>
+#define leftMotorPWMPin 6
-Listă cu documente, datasheet-uri, resurse Internet folosite, eventual grupate pe **Resurse Software** şi **Resurse Hardware**.
+#define leftMotorDirPin 7
-</note>
+#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);
+    motorPower = Kp * error + Ki * errorSum * sampleTime - Kd * (currentAngle - prevAngle) / sampleTime;
+    prevAngle = currentAngle;
-<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));
+    }
+}