Get best of the week

Implementasi suatu algoritma untuk menentukan lebar suatu objek menggunakan pengintai ultrasonik dan penyandi dalam bahasa RobotC

Selamat siang!

Hari ini saya ingin berbagi implementasi algoritma untuk menentukan lebar suatu objek menggunakan pengintai ultrasonik dan encoder dalam bahasa pemrograman RobotC (menggunakan platform VEX EDR).

Prasejarah


Baru-baru ini, saya ditawari untuk menyelenggarakan kelas master pada pemecahan masalah yang menarik menggunakan bahasa pemrograman RobotC. Selama kelas master, saya mempertimbangkan tugas menentukan lebar objek, implementasi algoritma yang ingin saya bagikan dalam publikasi ini.

Basis unsur


Sebagai tempat pelatihan eksperimental, saya menggunakan lingkungan Robot Dunia Maya untuk platform VEX, model robot virtual VEX Launcher, model virtual elemen kompetisi VRC Turning Point (Gambar 1) dan rumus matematika paling sederhana (untuk menentukan panjang akor dalam lingkaran dengan sudut tengah yang diketahui).

gambar
Gambar 1 . Tempat pelatihan virtual dengan robot

Tugas


- Menentukan diameter bola hanya menggunakan pengintai ultrasonik dan encoders yang dipasang pada robot

Landasan teori


Untuk menentukan lebar objek, saya menggunakan konsep pola radiasi (Gambar 2).

gambar
Gambar 2. Sensitivitas dan pola arah dari pencari rentang ultrasonik

Pada intinya, perlu untuk menentukan sudut yang terbentuk ketika beralih dari posisi kanan ekstrem ke posisi kiri ekstrim antara tangen pada titik ekstrim diagram.
Pertimbangkan Gambar 3.

gambar
Gambar 3 . Algoritma geometri

Kami akan mencari sudut tengah busur berdasarkan hubungan antara jari-jari sudut dan busur rotasi robot. Nilai lebar akhir akan sesuai dengan kord busur.

Parameter input


Sebagai parameter input, saya telah memilih:

  • dist = 100 mm (jarak di mana robot berhenti mengukur, parameter dipilih berdasarkan keberadaan benda lain di sekitar objek yang mungkin jatuh ke bidang pandang sensor dan mengganggu pengukuran)
  • d_wheel = 102 mm (diameter roda robot)
  • d_ball = 102 mm (diameter bola aktual)

Kesalahan ditentukan sebagai parameter output dari tugas ini, yang akan menunjukkan keakuratan metode kami dan parameter yang dipilih:

  • error = abs (d_ball_exper - d_ball)

Algoritma Operasi Program


Tahap-tahap algoritma berikut diidentifikasi.

  1. Gerakan ke objek.
  2. Putar ke titik paling kanan dari suatu objek.
  3. Putar ke titik paling kiri dari objek.
  4. Perhitungan sudut menggunakan bacaan yang diambil dari encoders.
  5. Perhitungan kesalahan.

Implementasi algoritma RobotC


Pertama-tama, kita perlu menginisialisasi konfigurasi standar robot virtual:

#pragma config(StandardModel, "VEX Launchbot")

Kami menerapkan fungsi yang akan bertanggung jawab untuk mengubah robot ke posisi paling kanan dan / atau ekstrim kiri. Fungsi mengembalikan encoder setelah belokan, yang akan digunakan dalam perhitungan lebih lanjut:

int turn_to_over_object(int leftSpeed, int rightSpeed, int distance, bool need_object, bool left_side)
{
if(need_object == true)
{
if(left_side == true)
{
while(SensorValue[sonarSensor] > distance)
{
setMultipleMotors(-leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(rightSpeed, rightMotor,frontRightMotor);
}
}
else
{
while(SensorValue[sonarSensor] > distance)
{
setMultipleMotors(leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(-rightSpeed, rightMotor,frontRightMotor);
}
}
}
else
{
if(left_side == true)
{
while(SensorValue[sonarSensor] < (distance + 1))
{
setMultipleMotors(-leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(rightSpeed, rightMotor,frontRightMotor);
}
}
else
{
while(SensorValue[sonarSensor] < (distance + 1))
{
setMultipleMotors(leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(-rightSpeed, rightMotor,frontRightMotor);
}
}
}

return getMotorEncoder(leftMotor);
}

Mari kita lihat lebih dekat argumen fungsi:

  • leftSpeed ​​- kecepatan drive kiri
  • rightSpeed ​​- kecepatan drive yang tepat
  • rightSpeed ​​- kecepatan drive yang tepat
  • jarak - jarak ke objek tempat robot berhenti
  • need_object - mode gerak: Benar, jika suatu objek diperlukan di bidang sensor, Salah jika objek diperlukan
  • left_side - arah rotasi robot. Benar - belok kiri, Salah - belok kanan

Dengan nilai need_object = True, robot akan berputar hingga objek berada jauh dari robot (dalam kasus kami, 100 mm).

Dengan nilai need_object = False, robot akan berputar hingga objek berada pada jarak (jarak +1) dari robot. Parameter unit secara total mempengaruhi keakuratan akhir dalam menentukan tidak adanya objek dalam bidang pandang sensor (dimungkinkan untuk meningkatkan program dengan mengambil parameter ini ke dalam argumen fungsi).

Mari kita menulis bagian utama dari program ini:

task main()
{
int d_ball = 10.2; //sm
int d_wheel = 10.2;

int dist = 10; //sm
//  
setMotor(armMotor, -50);
wait(1);

while(SensorValue[sonarSensor] > 10)
{
setMultipleMotors(50, leftMotor, rightMotor, frontLeftMotor, frontRightMotor);
}
stopAllMotors();
int enc = 0;
enc = turn_to_over_object(50, 50, dist, false, false);
enc = turn_to_over_object(50, 50, dist, true, true);
resetMotorEncoder(leftMotor);
enc = turn_to_over_object(50, 50, dist, false, true);
stopAllMotors();

float teta = abs(enc) * PI * d_wheel * d_wheel / (dist * 627);
float d_ball_exper = 2 * dist * sin(teta / 2);
	
float error = abs(d_ball_exper - d_ball);
while(1)
{
}
}

Loop sementara pertama berjalan hingga robot mencapai jarak yang ditentukan ke objek. Selanjutnya, kita perlu memanggil fungsi yang ditulis di atas satu per satu untuk mengukur pembacaan enkoder antara posisi paling kanan dan paling kiri (perhatikan bahwa sebelum mengukurnya perlu mengatur ulang enkoder ke nol).

Untuk mendapatkan rumus sudut, saya dipandu oleh rumus berikut:
- enc = N * 627, di mana
    N adalah jumlah putaran roda,
    enc adalah pembacaan encoder,
    627 adalah resolusi encoder => express N;
- S = N * l, di mana
    l adalah panjang roda, l = PI * d_wheel * d_wheel => kita mengganti N dan l => S = enc * PI * d_wheel * d_wheel / 627;
- di sisi lain, S = dist * teta, di mana
    S adalah besarnya busur yang dihidupkan robot,
    teta adalah sudut tengah yang diinginkan;
- berdasarkan data di atas, kami menyatakan teta => teta = enc * PI * d_wheel * d_wheel / (627 * dist)

Untuk menemukan chord yang sama dengan diameter bola yang diinginkan, kami menggunakan rumus:
    d_ball_exper = 2 * dist * sin (teta / 2)

Loop sementara tak berujung pada akhir program diperlukan untuk melihat nilai yang diterima dalam debugger.

Data ringkasan


Nilai diameter bola dan kesalahan berikut diperoleh pada bidang virtual:

    d_ball_exper = 10,6 cm
    kesalahan = 0,4 cm

Seperti yang Anda lihat, metode yang diperoleh untuk mengukur lebar memberikan nilai kesalahan yang kecil, yang dapat diterima untuk kelas akurasi sensor yang digunakan. Nilai kesalahan dapat dikurangi dengan memilih nilai parameter dist (misalnya, memulai siklus dengan robot bergerak kembali dengan langkah tertentu yang berubah menjadi dist) atau menggunakan pengontrol PID, yang akan mengurangi pengaruh inersia robot pada nilai akhir encoder selama pengereman.

Prospek


Di masa depan, saya akan mempertimbangkan metode lain menggunakan sensor robot untuk menentukan parameter lingkungan.

More articles:


All Articles