Robot Ramah Pelacakan Orang Omni-Directional

Kode

• Sketsa Arduino - Utama
• Arduino Sketch - Kontrol Motor
• C++ Pelacakan Wajah Untuk Ci20

Arduino Sketch - UtamaArduino

// Motor 1int dir1PinA =3;int dir2PinA =2;int speedPinA =9; // Perlu pin PWM untuk dapat mengontrol kecepatan motor// Motor 2int dir1PinB =4;int dir2PinB =5;int speedPinB =8; // Perlu pin PWM untuk dapat mengontrol kecepatan motor// Motor 3int dir1PinC =6;int dir2PinC =7;int speedPinC =10; // Perlu pin PWM untuk dapat mengontrol kecepatan motorint x =0;int y =0;int dominanUltrasonic =0;bool moveMotor =false;int startTime =0;void setup() { Serial.begin(9600); pinMode(dir1PinA,OUTPUT); pinMode(dir2PinA,OUTPUT); pinMode (kecepatanPinA, OUTPUT); pinMode(dir1PinB,OUTPUT); pinMode(dir2PinB,OUTPUT); pinMode (kecepatanPinB, OUTPUT); pinMode(dir1PinC,OUTPUT); pinMode(dir2PinC,OUTPUT); pinMode(speedPinC,OUTPUT); startTime =milis();}String rpiString;void loop() { readDataFromRPi(); // Ini menguji apakah kita telah menerima nilai dari RPi dalam detik terakhir. // Ini secara efektif bertindak sebagai buffer sehingga tidak terus berhenti &mulai karena perintah move tidak kontinu // Ini juga memungkinkan penghentian jika tidak ada yang masuk dari RPi// int elapsedTime =millis() - startTime; // if (waktu berlalu> 1000)// {// x =0;// y =0;// dominanUltrasonic =0;// moveMotor =false;// startTime =milis();// // } // Kirim X &Y ke motor //if(moveMotor ==true &&(x !=0 &&y !=0)) //{ //Serial.println("MovingMotor"); // driveInDirection(x,y); //} //if(x ==0 &&y ==0) //{ //Serial.println("ZeroMMotor"); // driveInDirection(x,y); //} // Ini mungkin hanya tersisa dari ultrasonik - ragu-ragu untuk menghapus delay(30);}void readDataFromRPi(){ // Baca dari Rpi while (Serial.available()) { delay(3); //delay untuk mengizinkan buffer terisi if (Serial.available()>0) { char c =Serial.read(); //mendapat satu byte dari buffer serial rpiString +=c; //membuat string readString if(c =='n') { break; } } } // ENDWHILE // Jika sesuatu telah dibaca dari RPi maka masukkan ke x,y &domniantUltrasonic if (rpiString.length()>0) { Serial.println(rpiString); //melihat apa yang diterima String isRotate =getValue(rpiString, ' ',0); String xval =getValue(rpiString, ' ', 1); String yval =getValue(rpiString, ' ', 2); x =xval.toInt(); y =yval.toInt(); waktu mulai =mili(); if (isRotate =="r") { rotate(x); } else { driveInDirection(x,y); } rpiString=""; } //ENDIF} String getValue(Data string, pemisah karakter, indeks int){ int ditemukan =0; int strIndeks[] ={0, -1 }; int maxIndex =data.length()-1; for(int i=0; i<=maxIndex &&ditemukan<=index; i++){ if(data.charAt(i)==separator || i==maxIndex){ ditemukan++; strIndeks[0] =strIndeks[1]+1; strIndex[1] =(i ==maxIndex) ? saya+1 :saya; } } kembali ditemukan>indeks ? data.substring(strIndex[0], strIndex[1]) :"";}

Arduino Sketch - MotorControlArduino

/* * Kode kontrol motor * * Kelas ini akan menyertakan kode untuk membuat robot bergerak ke segala arah * dan berputar di sekitar titik tengah. */void driveInDirection(float newX, float newY){ delay(20); float x =baruX; mengapung y =baruY; float theta =atan2(y,x); float mag =kuadrat((x*x) + (y*y)); float vx =mag * cos(theta); float vy =mag * sin(theta); mengapung w1 =-vx; float w2 =0,5 * vx - kuadrat(3)/2 * vy; float w3 =0,5 * vx + kuadrat(3)/2 * vy; // Dapatkan nilai w terbesar float wSet[] ={w1, w2, w3}; float terbesarNilai =0,0; for (int i =0; i <3; i++) { if(abs(wSet[i])> terbesarValue) { terbesarValue =abs(wSet[i]); } } float speedCoef =(float)147.0 / nilai terbesar; w1 =w1 * kecepatanCoef; w2 =w2 * kecepatanCoef; w3 =w3 * kecepatanCoef; if (x ==0 &&y ==0) { w1 =0; w2 =0; w3 =0; } Serial.println(w1); Serial.println(w2); Serial.println(w3); w1 =kendala(w1, -150, 150); w2 =kendala(w2, -150, 150); w3 =kendala(w3, -150, 150); boolean w1_ccw =w1 <0 ? benar salah; boolean w2_ccw =w2 <0 ? benar salah; boolean w3_ccw =w3 <0 ? benar salah; byte w1_speed =(byte) peta(abs(w1), 0, 150, 0, 255); byte w2_speed =(byte) peta(abs(w2), 0, 150, 0, 255); byte w3_speed =(byte) peta(abs(w3), 0, 150, 0, 255); printMotorSpeed(w1_speed, 1); printMotorSpeed(w2_speed, 2); printMotorSpeed(w3_speed, 3); analogWrite(speedPinA, w1_speed);//Menyetel variabel kecepatan melalui PWM analogWrite(speedPinB, w2_speed); analogWrite(speedPinC, w3_speed);//Menyetel variabel kecepatan melalui PWM digitalWrite(dir1PinA, !w1_ccw); digitalWrite(dir2PinA, w1_ccw); digitalWrite(dir1PinB, !w2_ccw); digitalWrite(dir2PinB, w2_ccw); digitalWrite(dir1PinC, w3_ccw); digitalWrite(dir2PinC, !w3_ccw);}void rotate(float milidetik){ float w1 =255; mengapung w2 =255; mengapung w3 =255; boolean w1_ccw =w1 <0 ? benar salah; boolean w2_ccw =w2 <0 ? benar salah; boolean w3_ccw =w3 <0 ? benar salah; byte w1_speed =(byte) peta(abs(w1), 0, 150, 0, 255); byte w2_speed =(byte) peta(abs(w2), 0, 150, 0, 255); byte w3_speed =(byte) peta(abs(w3), 0, 150, 0, 255); printMotorSpeed(w1_speed, 1); printMotorSpeed(w2_speed, 2); printMotorSpeed(w3_speed, 3); analogWrite(speedPinA, w1_speed);//Menyetel variabel kecepatan melalui PWM analogWrite(speedPinB, w2_speed); analogWrite(speedPinC, w3_speed);//Menyetel variabel kecepatan melalui PWM digitalWrite(dir1PinA, !w1_ccw); digitalWrite(dir2PinA, w1_ccw); digitalWrite(dir1PinB, !w2_ccw); digitalWrite(dir2PinB, w2_ccw); digitalWrite(dir1PinC, w3_ccw); digitalWrite(dir2PinC, !w3_ccw); penundaan (milidetik); analogWrite(speedPinA, 0);//Menyetel variabel kecepatan melalui PWM analogWrite(speedPinB, 0); analogWrite(speedPinC, 0);//Menyetel variabel kecepatan melalui PWM }void printMotorSpeed(byte motorSpeed, int motor){ Serial.print("Motor"); Serial.print(motor); Serial.print(":"); Serial.println(Kecepatan motor); }

C++ Pelacakan Wajah Untuk Ci20C/C++

#include "opencv2/objdetect/objdetect.hpp"#include "opencv2/highgui/highgui.hpp"#include "opencv2/imgproc/imgproc.hpp"#include #include menggunakan namespace std;menggunakan namespace cv;CascadeClassifier face_cascade, eyes_cascade;String window_name ="Face Detection";#include #include #include #include #include #include #include #include int sendSerial(char* message){int fd =open("/ dev/ttyUSB0", O_RDWR);if (fd ==-1){ perror("dev/ttyUSB0"); return 1;}struct termios tios;tcgetattr(fd, &tios);tios.c_iflag =IGNBRK | IGNPAR;tios.c_oflag =0;tios.c_lflag =0;cfsetspeed(&tios, B9600);tcsetattr(fd, TCSAFLUSH,&tios);usleep(1000);//char msg[] ="50 50";write(fd , message, strlen(message));return 0;}/** * Mendeteksi wajah dan menggambar elips di sekitarnya */void detectFaces(Mat frame) { std::vector wajah; Tikar bingkai_abu-abu; // Konversi ke skala abu-abu cvtColor(frame, frame_gray, COLOR_BGR2GRAY); // Equalize histogram equalizeHist(frame_gray, frame_gray); // Deteksi wajah face_cascade.detectMultiScale(frame_gray, face, 1.1, 3, 0|CASCADE_SCALE_IMAGE, Ukuran(30,30)); // Ulangi semua wajah untuk(size_t i =0; i ; // Mencoba mendeteksi mata, di dalam setiap wajah // eyes_cascade.detectMultiScale(face, eyes, 1.1, 2, // 0 |CASCADE_SCALE_IMAGE, Size(50, 50) ); // if(eyes.size()> 0) // Menggambar elips di sekitar wajah elips(bingkai, tengah, Ukuran(wajah[i].lebar/2, wajah[i].tinggi/2), 0, 0, 360 , Skalar( 255, 0, 255 ), 4, 8, 0 ); if(center.x> frame.cols/3 &¢er.x =0) // pause break; } kembalikan 0;}

Skema

Lihat deskripsi yang lebih baik di bagian cerita