MIRO adalah platform robot terbuka dalam ruangan. Bagian 5 - Komponen perangkat lunak: ARDUINO (AVR), kami memanjat "di bawah tenda"

Kali ini kita akan melihat sedikit lebih dalam tentang implementasi beberapa metode pustaka kunci untuk ARDUINO (AVR), yang bertanggung jawab untuk memindahkan robot MIRO. Bagian ini akan menarik bagi semua orang yang bertanya-tanya bagaimana mengontrol kecepatan linear dan sudut robot pada ARDUINO, dilengkapi dengan motor dengan enkoder yang paling sederhana.

Daftar Isi: Bagian 1 , Bagian 2 , Bagian 3 , Bagian 4 , Bagian 5 .

Metode yang bertanggung jawab untuk mengemudi dengan odometry masih menyakitkan dalam hal menjelaskan bagaimana, apa, dan mengapa. Hal pertama yang perlu Anda ketahui tentang mengendalikan pergerakan robot adalah fakta sederhana dan jelas bahwa motor pengumpul robot tidak pernah berputar pada kecepatan yang sama tanpa penyesuaian tambahan. Kopling yang berbeda, karakteristik keluaran yang berbeda dari saluran driver, motor listrik yang sedikit berbeda dan pelumasan di gearbox.

Fakta kedua yang harus Anda pahami dan ketahui adalah kehadiran inersia di mesin, bahkan dengan rasio gigi yang cukup besar. Yaitu ketika melepaskan tegangan dari terminal motor, roda, bahkan tidak dimuat, membuat gerakan beberapa derajat lagi. Besarnya rotasi tambahan ini tergantung pada gaya pemuatan pada roda, pada kecepatan rotasi sebelum menghilangkan stres, dan pada faktor-faktor tak kasat mata yang sama seperti jenis dan jumlah pelumas dalam gearbox.

Fakta-fakta ini menentukan implementasi dari sekelompok metode yang berkaitan dengan pergerakan sasis yang dilengkapi dengan sensor odometer (dalam kasus MIRO, penyandi digital dari setiap roda).

Seperti yang kami temukan di bagian keempat, dalam model perangkat lunak ada kelas Chassis , yang menerapkan kontrol rotasi mesin sasis individu. Saya ingin menekankan - bukan kontrol gerakan sasis, troli, tetapi kontrol mesin troli. Kontrol langsung troli diterapkan di kelas Robot dan Miro .

Mari kita mulai dari atas. Di bawah ini adalah metode kelas Miro yang mengimplementasikan pergerakan robot pada jarak tertentu ( dist , meter) dengan kecepatan linier ( lin_speed , m / s) dan angular ( ang_speed , deg / s). Kami belum memperhatikan parameter en_break .

int Miro::moveDist(float lin_speed, float ang_speed, float dist, bool en_break) { float _wheelSetAngSpeed[WHEEL_COUNT]; _wheelSetAngSpeed[LEFT] = MIRO_PI2ANG * (lin_speed - (ROBOT_DIAMETER * ang_speed / (2 * MIRO_PI2ANG))) / WHEEL_RADIUS; _wheelSetAngSpeed[RIGHT] = MIRO_PI2ANG * (lin_speed + (ROBOT_DIAMETER * ang_speed / (2 * MIRO_PI2ANG))) / WHEEL_RADIUS; float _wheelSetAng[WHEEL_COUNT]; _wheelSetAng[RIGHT] = _wheelSetAngSpeed[RIGHT] * dist / lin_speed; _wheelSetAng[LEFT] = _wheelSetAngSpeed[LEFT] * dist / lin_speed; return this->chassis.wheelRotateAng(_wheelSetAngSpeed, _wheelSetAng, en_break); } 

Dalam metode ini, kecepatan sudut DIBUTUHKAN untuk mesin kiri dan kanan pertama kali dihitung. Menurut formula yang cukup jelas, yang tidak masalah untuk disimpulkan. Hanya perlu diingat bahwa kecepatan linear dalam metode ini ditentukan dalam meter per detik, dan kecepatan sudut dalam derajat per detik (bukan dalam radian). Oleh karena itu, kami menghitung sebelumnya konstanta MIRO_PI2ANG = 57.29 = 180 / pi. ROBOT_DIAMETER - jarak antara roda kiri dan kanan robot (dalam meter), WHEEL_RADIUS - jari-jari roda (juga dalam meter). Semua konstanta numerik untuk kasus tersebut terkandung dalam file defs.h, dan parameter khusus robot dan sasis ada di file config.h.

Setelah itu, sudut dihitung dengan mana setiap roda harus diputar sehingga robot menempuh jarak dist (juga dalam meter).

Jadi, pada tahap ini kita mendapatkan kecepatan dan sudut apa yang Anda butuhkan untuk memutar setiap roda sasis robot. Dan kemudian metode wheelRotateAng () dari objek sasis disebut.

Metode wheelRotateAng (kecepatan float *, float * ang, bool en_break) digunakan untuk memutar roda robot dengan kecepatan sudut yang ditentukan oleh array kecepatan [] (dalam m / s) dengan sudut yang ditentukan oleh larik ang [] (dalam derajat). Parameter terakhir en_break (sudah kami temui sebelumnya) menetapkan persyaratan untuk pemberhentian roda setelah berbelok dengan menerapkan tegangan balik jangka pendek untuknya. Ini diperlukan untuk menekan inersia robot, mencegahnya bergerak melampaui jarak yang diperlukan setelah melepas tegangan kontrol dari motor. Untuk kepuasan penuh, tentu saja, ada metode wheelRotateAngRad () , mirip dengan wheelRotateAng () dengan perbedaan bahwa dibutuhkan nilai sudut rotasi dan kecepatan sudut dalam radian dan radian per detik sebagai parameter.

Algoritma metode wheelRotateAng () adalah sebagai berikut.

1. Pertama, korespondensi nilai dari kecepatan [] dan ang [] untuk beberapa kondisi batas diperiksa. Jelas, sasis memiliki keterbatasan fisik baik pada kecepatan sudut maksimum rotasi roda, dan pada minimum (kecepatan minimum menjauh). Juga, sudut dalam ang [] tidak boleh kurang dari sudut tetap minimum rotasi, ditentukan oleh keakuratan pembuat enkode.

2. Selanjutnya, arah rotasi setiap roda dihitung. Jelas melalui tanda produk dan [i] * kecepatan [i] ;

3. “Jarak rotasi” Dw [i] untuk setiap roda dihitung - jumlah sampel enkoder yang harus dilakukan untuk diputar oleh ang [i] yang diberikan .
Nilai ini ditentukan oleh rumus:

Dw [i] = ang [i] * WHEEL_SEGMENTS / 360 ,
di mana WHEEL_SEGMENTS adalah jumlah segmen dari roda enkoder (revolusi penuh).

4. Nilai tegangan pada driver motor dicatat.


5. Menurut tabel kalibrasi untuk setiap roda, nilai awal sinyal PWM ditentukan, yang memastikan rotasi roda dengan kecepatan kecepatan yang diperlukan [i] . Jenis tabel kalibrasi dan dari mana asalnya - kami akan menganalisis lebih lanjut.

6. Rotasi mesin dimulai sesuai dengan nilai kecepatan dan arah putaran yang dihitung. Dalam teks implementasi kelas, metode pribadi _wheel_rotate_sync () bertanggung jawab untuk ini.

Kami melangkah lebih dalam. Metode _wheel_rotate_sync () bekerja sesuai dengan algoritma berikut:

1. Dalam loop tak terbatas, pemeriksaan dilakukan untuk mencapai penghitung respons enkoder dari jarak belok Dw [i] untuk setiap roda. Jika salah satu dari penghitung Dw [i] tercapai, semua roda berhenti dan keluar dari siklus dan kemudian keluar dari fungsi (langkah 5). Ini dilakukan karena alasan berikut. Karena perbedaan pengukuran sudut rotasi, itu adalah situasi yang sangat umum ketika jarak terhitung Dw [i] dari satu roda diperoleh dengan membulatkan nilai non-integer ke sisi yang lebih kecil dan Dw [j] dari roda kedua ke yang lebih besar. Ini mengarah pada fakta bahwa setelah menghentikan salah satu roda, roda kedua terus berbelok. Untuk sasis dengan penggerak diferensial (dan untuk banyak lainnya), ini mengarah pada "pergantian" robot yang tidak terencana di akhir tugas. Karena itu, dalam hal mengatur pergerakan spasial dari seluruh sasis, perlu untuk menghentikan semua mesin sekaligus.

2. Jika Dw [i] tidak tercapai, maka di loop fakta operasi encoder berikutnya diperiksa (variabel _syncloop [w] , diperbarui dari interupsi encoder dan reset di loop infinite ini). Ketika persimpangan berikutnya terjadi, program menghitung modul kecepatan sudut saat ini setiap roda (deg / s), sesuai dengan rumus yang jelas:

W [i] = (360 * tau [i]) / WHEEL_SEGMENTS ,
dimana:
tau [i] - nilai rata-rata waktu antara dua respons terakhir dari pembuat enkode. "Kedalaman" dari filter rata-rata ditentukan oleh MEAN_DEPTH dan default ke 8.

3. Berdasarkan kecepatan roda yang dihitung, kesalahan absolut dihitung sebagai perbedaan antara set dan kecepatan sudut aktual.

4. Berdasarkan kesalahan yang dihitung, aksi kontrol (nilai sinyal PWM) dikoreksi untuk setiap motor.

5. Setelah mencapai Dw [i] , dalam kasus en_break aktif, tegangan balik jangka pendek diterapkan ke motor. Durasi efek ini ditentukan dari tabel kalibrasi (lihat di bawah) dan biasanya berkisar antara 15 hingga 40 ms.

6. Ada rilis lengkap stres dari mesin dan keluar _wheel_rotate_sync () .

Saya sudah menyebutkan tabel kalibrasi tertentu dua kali. Jadi, di perpustakaan ada tabel khusus nilai yang disimpan di EEPROM memori robot dan berisi rekaman tiga nilai terkait:

1. Tegangan di terminal motor. Ini dihitung dengan menerjemahkan nilai sinyal PWM ke tegangan aktual. Untuk ini, pada langkah 4 metode wheelRotateAng () , tegangan aktual pada driver mesin dicatat.

2. Kecepatan sudut rotasi roda (tanpa beban) sesuai dengan tegangan yang diberikan.

3. Durasi sinyal henti sesuai dengan kecepatan sudut ini.
Secara default, ukuran tabel kalibrasi adalah 10 catatan (ditentukan oleh WHEEL_TABLE_SIZE konstan dalam file config.h ) - 10 tiga kali lipat dari nilai "tegangan - kecepatan sudut - durasi sinyal berhenti".

Untuk menentukan nilai dari entri 2 dan 3 dalam tabel ini, metode khusus digunakan - wheelCalibrate (byte wheel) .

Mari kita lihat sedikit ke dalamnya. Metode ini mengimplementasikan serangkaian tindakan untuk menentukan nilai yang hilang dalam tabel kalibrasi engine / roda, serta untuk mengetahui kecepatan sudut minimum mulai dan kecepatan sudut maksimum roda.

Untuk melakukan kalibrasi, robot dipasang pada dudukan, semua rotasi roda selama kalibrasi dilakukan tanpa beban.

1. Pertama, Anda perlu menentukan kecepatan mulai minimum. Ini dilakukan dengan sangat sederhana. Dalam sebuah siklus, PWM kontrol diumpankan ke mesin, mulai dari 0, dengan kenaikan 1. Pada setiap langkah, program menunggu beberapa waktu, ditentukan oleh WHEEL_TIME_MAX konstan ( penundaan normal () ). Setelah waktu tunggu berlalu, ia memeriksa apakah permulaan telah selesai (dengan mengubah nilai penghitung pembuat enkode). Jika pullaway selesai, maka kecepatan sudut rotasi roda dihitung. Untuk kepastian yang lebih besar, nilai 10 ditambahkan ke nilai PWM yang sesuai dengan kecepatan awal ini. Ini memberikan pasangan pertama nilai "tegangan pada motor" - "kecepatan sudut".

2. Setelah kecepatan mulai ditemukan, langkah PWM dihitung untuk mengisi tabel kalibrasi secara seragam.

3. Dalam siklus, untuk setiap nilai PWM baru, roda diputar 2 putaran penuh dan kecepatan sudut diukur sesuai dengan algoritma yang mirip dengan metode _wheel_rotate_sync () . Dalam siklus yang sama, juga dengan pendekatan yang berurutan, nilai optimal dari durasi sinyal berhenti diukur. Awalnya, beberapa nilai jelas diambil. Dan kemudian diuji dalam mode "turn-stop". Sebagai yang optimal, nilai maksimum durasi sinyal stop dipilih, di mana "putaran jarak" yang ditetapkan tidak terlampaui. Dengan kata lain, nilai durasi sinyal seperti itu, pada saat pasokan ke mesin di satu sisi, inersia ditekan, dan di sisi lain, tidak ada gerakan mundur jangka pendek (yang difiksasi oleh enkoder yang sama).

4. Setelah kalibrasi selesai, tegangan kontrol ke motor yang dikalibrasi berhenti diterapkan dan tabel kalibrasi roda ini dicatat dalam EEPROM.

Saya menghilangkan segala macam hal sepele dari implementasi dan mencoba untuk menyatakan esensinya. Anda mungkin memperhatikan bahwa metode wheelRotateAng () dan wheelRotateAngRad () menghalangi fungsi. Ini adalah harga untuk akurasi gerakan dan integrasi yang cukup sederhana ke dalam sketsa pengguna. Dimungkinkan untuk membuat task manager kecil dengan timing tetap, tetapi ini akan mengharuskan pengguna untuk menanamkan fungsionalitas mereka secara ketat dalam kuota waktu yang diberikan.

Dan untuk aplikasi non-blocking, API memiliki fungsi wheelRotate (float * speed) . Itu, seperti yang dapat dilihat dari daftar parameter, cukup melakukan rotasi roda dengan kecepatan yang ditetapkan. Dan kecepatan rotasi disesuaikan dalam metode Sync () pada sasis robot, yang disebut dalam metode Sync () dari objek kelas Miro dengan nama yang sama. Dan sesuai dengan persyaratan untuk struktur sketsa pengguna, metode ini harus disebut setiap iterasi dari loop utama () loop dari sketsa ARDUINO.

Pada langkah 4, dalam uraian metode _wheel_rotate_sync () , saya menyebutkan "koreksi kontrol" dari mesin. Bagaimana menurut Anda)? Ini adalah kontroler PID). Nah, lebih tepatnya kontroler PD. Seperti yang Anda tahu (pada kenyataannya - tidak selalu), cara terbaik untuk menentukan koefisien regulator adalah seleksi). Ada satu definisi dalam file konfigurasi config.h:

 #define DEBUG_WHEEL_PID 

Jika Anda membatalkan komentarnya, maka ketika Anda memanggil metode moveDist () dari kelas Miro, grafik terbalik berikut dari kesalahan relatif dalam mengendalikan kecepatan sudut salah satu roda robot (kiri) akan ditampilkan di konsol robot.



Tidak menyerupai apa pun)? Turun adalah waktu (setiap batang adalah langkah dari siklus kontrol), dan nilai kesalahan disimpan ke kanan (dengan tanda dipertahankan). Berikut adalah dua pasang grafik pada skala yang sama dengan koefisien yang berbeda dari pengontrol PD. "Humps" hanyalah "gelombang" dari overshoot. Angka-angka pada bilah horizontal adalah kesalahan relatif (dengan pelestarian tanda). Visualisasi sederhana dari regulator, membantu menyesuaikan koefisien secara manual. Seiring waktu, saya berharap untuk melakukan pengaturan otomatis, tetapi untuk sekarang.

Berikut adok seperti itu :-)

Terakhir, mari kita lihat sebuah contoh. Langsung dari perpustakaan API_Miro_moveDist:

 #include <Miro.h> using namespace miro; byte PWM_pins[2] = { 5, 6 }; byte DIR_pins[2] = { 4, 7 }; byte ENCODER_pins[2] = { 2, 3 }; Miro robot(PWM_pins, DIR_pins, ENCODER_pins); int laps = 0; void setup() { Serial.begin(115200); } void loop() { for (unsigned char i = 0; i < 4; i++) { robot.moveDist(robot.getOptLinSpeed(), 0, 1, true); delay(500); robot.rotateAng(0.5*robot.getOptAngSpeed(), -90, true); delay(500); } Serial.print("Laps: "); Serial.println(laps); laps++; } 

Dari teks program semuanya harus jelas. Cara kerjanya - dalam video.


600 x 600 mm ubin dan celah ubin 5 mm. Secara teori, robot harus mengelilingi kotak dengan sisi 1 meter. Tentu saja, lintasan "melayang". Tetapi dalam keadilan layak untuk mengatakan bahwa dalam versi robot yang telah saya uji, ada mesin yang cukup berputar yang sulit untuk dikendarai dengan lambat. Tetapi pada kecepatan tinggi dan selip ada tempat untuk dituju, dan kelembaman tidak mudah untuk diatasi. Mesin dengan rasio roda gigi yang lebih tinggi (seperti bahkan pada robot MIRO kami, tidak tersedia selama pengujian) harus berperilaku agak lebih baik.

Jika ada saat-saat yang tidak dapat dipahami - Saya senang mengklarifikasi, mendiskusikan dan meningkatkan. Umpan balik umumnya menarik.