ESP32 DDMR Robot dengan FreeRTOS

Program Arduino IDE untuk robot Differential Drive Mobile Robot (DDMR) menggunakan ESP32 dengan sistem operasi real-time FreeRTOS. Program ini mengimplementasikan sistem odometri yang akurat dan navigasi dasar untuk robot beroda dua.

📋 Daftar Isi

• Gambaran Umum
• Spesifikasi Hardware
• Arsitektur Sistem
• Instalasi dan Setup
• Konfigurasi Pin
• Algoritma Odometri
• Sistem Navigasi
• Perintah yang Tersedia
• Kalibrasi
• Troubleshooting

🤖 Gambaran Umum

Program ini dirancang untuk robot DDMR (Differential Drive Mobile Robot) dengan fitur:

• Dual Core Processing: Core 0 untuk odometri dan navigasi, Core 1 untuk LED indicator
• Real-Time Odometry: Tracking posisi dan orientasi robot dengan presisi tinggi
• Navigation Functions: Fungsi maju dan belok dengan kontrol jarak dan sudut
• Thread-Safe: Menggunakan mutex untuk proteksi data yang dibagi antar task

🔧 Spesifikasi Hardware

Motor dan Gearbox

• Motor: JGA25-370 dengan encoder 2 fase (quadrature)
• Gear Ratio: 298:1 (dapat disesuaikan sesuai varian motor)
• Encoder Raw PPR: 11 pulses per revolution (sebelum gearbox)
• Effective PPR: 3,278 pulses per revolution roda (11 × 298)

Roda

• Diameter: 65mm
• Circumference: 204.2mm (π × 65mm)
• Resolusi Odometri: ~62 mikrometer per pulse

Motor Driver

• IC: TB6612FNG Dual Motor Driver
• Voltage: 2.5V - 13.5V (motor supply)
• Current: Hingga 1.2A per channel (3.2A peak)
• Logic Voltage: 3.3V (kompatibel dengan ESP32)

Mikrokontroller

• Board: ESP32 (dual-core Xtensa LX6)
• Core 0: Sistem odometri dan navigasi
• Core 1: LED blink dan task sekunder

🏗️ Arsitektur Sistem

FreeRTOS Tasks

Task 1: Odometry Task (Core 0)

• Prioritas: 2 (tinggi)
• Frekuensi: 50Hz (update setiap 20ms)
• Fungsi:
  • Membaca encoder counts
  • Menghitung odometri menggunakan kinematika differential drive
  • Update posisi robot (X, Y, θ)
  • Menampilkan pose robot setiap detik

Task 2: LED Task (Core 1)

• Prioritas: 1 (rendah)
• Frekuensi: 1Hz (blink setiap 500ms)
• Fungsi:
  • Blink LED D2 sebagai indikator sistem aktif

Interrupt Service Routines (ISR)

• Left Encoder ISR: Menghitung pulses encoder kiri
• Right Encoder ISR: Menghitung pulses encoder kanan
• Quadrature Decoding: Menentukan arah rotasi berdasarkan fase A dan B

🔌 Konfigurasi Pin

TB6612FNG Motor Driver

Pin ESP32  | TB6612FNG | Fungsi
-----------|-----------|------------------
25         | PWMA      | PWM Motor Kiri
26         | AIN1      | Direction 1 Motor Kiri  
27         | AIN2      | Direction 2 Motor Kiri
32         | PWMB      | PWM Motor Kanan
33         | BIN1      | Direction 1 Motor Kanan
14         | BIN2      | Direction 2 Motor Kanan
12         | STBY      | Standby (Enable Driver)

Encoder Connections

Pin ESP32  | Encoder   | Fungsi
-----------|-----------|------------------
18         | Left A    | Fase A Encoder Kiri
19         | Left B    | Fase B Encoder Kiri
21         | Right A   | Fase A Encoder Kanan
22         | Right B   | Fase B Encoder Kanan

LED Indicator

Pin ESP32  | Fungsi
-----------|------------------
2          | LED Built-in (D2)

📐 Algoritma Odometri

Prinsip Dasar

Program menggunakan kinematika differential drive untuk menghitung posisi dan orientasi robot berdasarkan rotasi kedua roda.

Rumus Matematika

1. Jarak yang Ditempuh Setiap Roda

left_distance = (ΔLeft_pulses × wheel_circumference) / effective_PPR
right_distance = (ΔRight_pulses × wheel_circumference) / effective_PPR

2. Gerakan Robot

Δdistance = (left_distance + right_distance) / 2
Δθ = (right_distance - left_distance) / wheelbase

3. Update Posisi

θ_new = θ_old + Δθ
θ_avg = (θ_old + θ_new) / 2
X_new = X_old + Δdistance × cos(θ_avg)  
Y_new = Y_old + Δdistance × sin(θ_avg)

Keunggulan Implementasi

• Average Heading: Menggunakan rata-rata sudut untuk update posisi yang lebih akurat
• Angle Normalization: Sudut selalu dinormalisasi ke range [-π, π]
• High Resolution: Resolusi ~62 mikrometer dengan gear ratio 298:1
• Thread-Safe: Mutex protection untuk akses data pose yang aman

🧭 Sistem Navigasi

Fungsi Maju (maju)

maju(1.5);  // Maju 1.5 meter

Algoritma:

1. Hitung jarak euclidean dari posisi start
2. Proportional control: kecepatan berkurang saat mendekati target
3. Stop otomatis saat mencapai jarak target

Fungsi Belok (belok)

belok(90);   // Belok kiri 90°
belok(-45);  // Belok kanan 45°

Algoritma:

1. Konversi derajat ke radian
2. Handle angle wrap-around untuk rotasi yang efisien
3. Proportional control: kecepatan putar berkurang mendekati target
4. Toleransi ±3° untuk presisi yang baik

🛠️ Instalasi dan Setup

Persyaratan Software

• Arduino IDE 1.8.19 atau lebih baru
• ESP32 Board Package
• Library yang diperlukan sudah built-in (FreeRTOS, Math)

Langkah Instalasi

1. Install ESP32 Board Package:

   • Buka Arduino IDE
   • File → Preferences
   • Tambahkan URL: https://dl.espressif.com/dl/package_esp32_index.json
   • Tools → Board → Boards Manager
   • Cari "ESP32" dan install

2. Upload Program:

   • Buka file esp32_ddmr_odometry.ino
   • Pilih Board: "ESP32 Dev Module"
   • Pilih Port yang sesuai
   • Upload program

3. Setup Hardware:

   • Hubungkan motor driver TB6612FNG sesuai skema pin
   • Hubungkan encoder ke pin interrupt ESP32
   • Pastikan power supply memadai (5V untuk motor, 3.3V untuk logic)

Wiring Diagram

ESP32           TB6612FNG       Motor/Encoder
------          ----------      -------------
3.3V    ───────   VCC           
GND     ───────   GND           
25      ───────   PWMA          
26      ───────   AIN1          
27      ───────   AIN2          ─────── Motor Kiri (+/-)
32      ───────   PWMB          
33      ───────   BIN1          
14      ───────   BIN2          ─────── Motor Kanan (+/-)
12      ───────   STBY          
        
18      ─────────────────────── Encoder Kiri A
19      ─────────────────────── Encoder Kiri B  
21      ─────────────────────── Encoder Kanan A
22      ─────────────────────── Encoder Kanan B

VM (Motor Power) ───── 6V-12V Supply

📟 Perintah yang Tersedia

Buka Serial Monitor (115200 baud) dan gunakan perintah berikut:

Perintah Navigasi

maju 2.5        # Maju 2.5 meter
belok 90        # Belok kiri 90°
belok -180      # Belok kanan 180°
stop            # Stop motor segera

Perintah Sistem

reset           # Reset posisi ke (0,0,0°)
info            # Tampilkan konfigurasi dan status robot

Contoh Output Serial Monitor

ESP32 DDMR Robot with FreeRTOS Starting...
Odometry task started on Core 0
LED task started on Core 1
Setup completed. Robot ready!

Motor Configuration:
- Wheel diameter: 65mm
- Gear ratio: 298:1
- Effective PPR: 3278

Available commands:
- 'maju <jarak_meter>' : Move forward
- 'belok <derajat>' : Turn (positive = left, negative = right)
- 'stop' : Stop motors
- 'reset' : Reset pose to origin
- 'info' : Show robot configuration

Pose - X: 0.000m, Y: 0.000m, Theta: 0.0°

⚙️ Kalibrasi

1. Kalibrasi Gear Ratio

Jika hasil gerakan tidak akurat:

#define GEAR_RATIO    298  // Sesuaikan dengan motor Anda

Cara test:

1. Reset pose: reset
2. Maju 1 meter: maju 1.0
3. Ukur jarak aktual dengan penggaris
4. Adjust gear ratio jika perlu

2. Kalibrasi Wheelbase

Untuk akurasi belokan:

#define WHEELBASE     0.15  // Jarak antar roda dalam meter

Cara test:

1. Reset pose: reset
2. Belok 360°: belok 360
3. Robot harus kembali ke orientasi awal
4. Adjust wheelbase jika ada drift

3. Kalibrasi Encoder Direction

Jika robot bergerak mundur saat perintah maju:

• Tukar kabel motor atau
• Inverse logika di ISR encoder

📊 Parameter Teknis

Resolusi Sistem

Gear Ratio:           298:1
Raw Encoder PPR:      11
Effective PPR:        3,278
Wheel Circumference:  204.2mm
Distance per Pulse:   62.3 μm
Angular Resolution:   0.11° (untuk wheelbase 150mm)

Performance

Odometry Update Rate: 50Hz (20ms interval)
Position Accuracy:    ±0.1mm (ideal conditions)
Angular Accuracy:     ±0.1° (ideal conditions)  
Response Time:        <20ms
Memory Usage:         ~6KB RAM

🔍 Detail Implementasi

Thread Safety

Program menggunakan mutex (poseMutex) untuk melindungi data pose robot yang diakses oleh multiple tasks:

if (xSemaphoreTake(poseMutex, portMAX_DELAY) == pdTRUE) {
    // Akses aman ke robotPose
    robotPose.x += deltaX;
    robotPose.y += deltaY; 
    robotPose.theta += deltaTheta;
    xSemaphoreGive(poseMutex);
}

Interrupt Handling

Encoder menggunakan hardware interrupt untuk capture setiap pulse:

void IRAM_ATTR leftEncoderISR() {
    if (digitalRead(ENCODER_LEFT_A) == digitalRead(ENCODER_LEFT_B)) {
        leftEncoderCount++;     // Forward rotation
    } else {
        leftEncoderCount--;     // Backward rotation
    }
}

Kinematika Differential Drive

Update posisi menggunakan average heading method untuk akurasi tinggi:

double avgTheta = (robotPose.theta + newTheta) / 2.0;
robotPose.x += deltaDistance * cos(avgTheta);
robotPose.y += deltaDistance * sin(avgTheta);

Proportional Control

Sistem navigasi menggunakan proportional controller sederhana:

// Untuk gerakan maju
if (remainingDistance < 0.1) {
    speed = (int)(speed * (remainingDistance / 0.1));
    speed = max(speed, 50);  // Minimum speed
}

// Untuk gerakan belok
if (abs(remainingAngle) < 0.2) {
    turnSpeed = (int)(turnSpeed * (abs(remainingAngle) / 0.2));
    turnSpeed = max(turnSpeed, 30);  // Minimum turn speed
}

📝 Struktur Data

Robot Pose

struct RobotPose {
    double x;        // Posisi X dalam meter
    double y;        // Posisi Y dalam meter  
    double theta;    // Orientasi dalam radian
};

Navigation State

volatile bool navigationActive = false;   // Flag navigasi aktif
volatile double targetDistance = 0.0;     // Target jarak (meter)
volatile double targetAngle = 0.0;        // Target sudut (radian)
volatile bool moveForward = false;        // Flag gerakan maju
volatile bool turnRobot = false;          // Flag gerakan belok

⚡ Real-Time Characteristics

Task Scheduling

• Odometry Task: Priority 2, 50Hz update rate
• LED Task: Priority 1, 1Hz blink rate
• Main Loop: Handle serial commands, 10Hz polling

Timing Requirements

• Encoder ISR: <1μs execution time (IRAM)
• Odometry Update: <1ms per cycle
• Navigation Control: 10ms update interval
• Serial Processing: 100ms polling interval

🧪 Testing dan Validasi

Test Odometri Dasar

1. Linear Movement Test:

   reset
   maju 1.0
   info          # Check if X ≈ 1.0m, Y ≈ 0.0m
   

2. Rotation Test:

   reset  
   belok 90
   info          # Check if θ ≈ 90°
   

3. Square Pattern Test:

   reset
   maju 1.0
   belok 90
   maju 1.0  
   belok 90
   maju 1.0
   belok 90
   maju 1.0
   belok 90
   info          # Should return close to origin
   

Expected Accuracy

• Linear accuracy: ±1cm untuk jarak 1-5 meter
• Angular accuracy: ±2° untuk rotasi 90-360°
• Repeatability: ±0.5cm untuk gerakan yang sama

🔧 Customization

Mengubah Gear Ratio

Untuk motor JGA25 varian lain:

// Varian umum JGA25-370:
#define GEAR_RATIO    120     // Untuk JGA25-370 (120:1)
#define GEAR_RATIO    298     // Untuk JGA25-370 (298:1) - default
#define GEAR_RATIO    495     // Untuk JGA25-370 (495:1)  
#define GEAR_RATIO    1000    // Untuk JGA25-370 (1000:1)

Mengubah Wheelbase

Sesuaikan dengan jarak fisik antar roda:

#define WHEELBASE     0.20    // 20cm wheelbase
#define WHEELBASE     0.15    // 15cm wheelbase - default

Mengubah Kecepatan

Adjust kecepatan di fungsi navigasi:

int speed = 150;        // Base speed (0-255)
int turnSpeed = 100;    // Turn speed (0-255)

🚨 Troubleshooting

Robot Bergerak Mundur saat Perintah Maju

Penyebab: Arah motor atau encoder terbalik Solusi:

1. Tukar kabel motor (+/-), atau
2. Inverse logika di ISR encoder, atau
3. Tukar pin AIN1-AIN2 atau BIN1-BIN2

Odometri Tidak Akurat

Penyebab: Parameter fisik tidak sesuai Solusi:

1. Verifikasi gear ratio motor
2. Ukur diameter roda yang sebenarnya
3. Ukur wheelbase dengan presisi
4. Cek koneksi encoder

Motor Tidak Berputar

Penyebab: Driver tidak enable atau koneksi salah Solusi:

1. Cek pin STBY terhubung ke pin 12
2. Pastikan power supply motor memadai (6-12V)
3. Cek koneksi PWM dan direction pins

Encoder Count Tidak Berubah

Penyebab: Interrupt tidak bekerja atau koneksi encoder Solusi:

1. Cek koneksi encoder A dan B
2. Pastikan menggunakan pin interrupt-capable
3. Periksa power supply encoder (biasanya 3.3V atau 5V)

Task Tidak Berjalan

Penyebab: Stack overflow atau priority conflict
Solusi:

1. Increase stack size di xTaskCreatePinnedToCore
2. Adjust task priority
3. Monitor free heap memory

💡 Tips Penggunaan

Optimasi Performa

1. Tuning PID: Implement PID controller untuk kontrol motor yang lebih smooth
2. Kalman Filter: Tambahkan sensor IMU dan gunakan Kalman filter untuk sensor fusion
3. Path Planning: Implement algoritma path planning untuk navigasi yang lebih complex

Monitoring dan Debug

info    # Tampilkan semua parameter dan status

Output akan menampilkan:

• Konfigurasi motor dan gearbox
• Resolusi sistem
• Posisi dan orientasi saat ini
• Nilai encoder real-time

Best Practices

1. Selalu reset pose sebelum test navigasi
2. Test gerakan kecil dahulu sebelum jarak jauh
3. Monitor serial output untuk debug
4. Pastikan surface yang rata untuk akurasi maksimal

🔬 Analisis Teknis

Error Sources

1. Slip roda: Dapat menyebabkan error akumulatif
2. Backlash gearbox: Error kecil saat perubahan arah
3. Quantization error: Terbatas oleh resolusi encoder
4. Timing jitter: Variasi timing ISR dapat mempengaruhi akurasi

Error Mitigation

1. Gunakan roda dengan grip baik
2. Kalibrasi parameter secara berkala
3. Implement sensor fusion dengan IMU
4. Use higher resolution encoders jika diperlukan

---

📄 Lisensi

Program ini dibuat untuk tujuan edukatif dan penelitian. Silakan gunakan dan modifikasi sesuai kebutuhan.

🤝 Kontribusi

Untuk improvement atau bug report, silakan buat issue atau pull request di repository ini.

---

Dibuat dengan ❤️ untuk komunitas robotika Indonesia @mochshultan at trkb unair