Omnidirectionele mensen volgen vriendelijke robot

Code

• Arduino Sketch - Hoofd
• Arduino Sketch - MotorControl
• FaceTracking C++ voor Ci20

Arduino Sketch - HoofdArduino

// Motor 1int dir1PinA =3;int dir2PinA =2;int speedPinA =9; // Moet een PWM-pin zijn om de motorsnelheid te kunnen regelen // Motor 2int dir1PinB =4;int dir2PinB =5;int speedPinB =8; // Moet een PWM-pin zijn om de motorsnelheid te kunnen regelen // Motor 3int dir1PinC =6;int dir2PinC =7;int speedPinC =10; // Moet een PWM-pin zijn om de motorsnelheid te kunnen regelen x =0;int y =0;int dominantUltrasonic =0;bool moveMotor =false;int startTime =0;void setup() {Serial.begin(9600); pinMode (dir1PinA, UITGANG); pinMode (dir2PinA, UITGANG); pinMode (speedPinA, UITGANG); pinMode (dir1PinB, UITGANG); pinMode (dir2PinB, UITGANG); pinMode (speedPinB, UITGANG); pinMode (dir1PinC, UITGANG); pinMode (dir2PinC, UITGANG); pinMode (speedPinC, UITGANG); startTime =millis();}String rpiString;void loop() { readDataFromRPi(); // Hiermee wordt getest of we in de laatste seconde een waarde van de RPi hebben ontvangen. // Het fungeert in feite als buffer zodat het niet blijft stoppen en starten omdat het verplaatscommando niet continu is // Het staat ook onderbreking toe als er niets binnenkomt van de RPi// int elapsedTime =millis() - startTime; // if (elapsedTime> 1000)// {// x =0;// y =0;// dominantUltrasonic =0;// moveMotor =false;// startTime =millis();// //} // Stuur de X &Y naar de motoren //if(moveMotor ==true &&(x !=0 &&y !=0)) //{ //Serial.println("MovingMotor"); // driveInDirection(x,y); //} //if(x ==0 &&y ==0) //{ //Serial.println("ZeroMMotor"); // driveInDirection(x,y); //} // Dit kan gewoon overblijven van de ultrasone trillingen - aarzelend om delay(30) te verwijderen;}void readDataFromRPi(){ // Read from Rpi while (Serial.available()) { delay(3); // vertraging om buffer te vullen als (Serial.available()>0) { char c =Serial.read(); // haalt één byte uit seriële buffer rpiString +=c; // maakt de string readString if (c =='n') {break; } } } // ENDWHILE // Als er iets van de RPi is gelezen, plaats het dan in x,y &domniantUltrasonic if (rpiString.length()>0) { Serial.println(rpiString); // kijk wat er is ontvangen String isRotate =getValue (rpiString, ' ',0); String xval =getValue(rpiString, ' ', 1); String yval =getValue(rpiString, ' ', 2); x =xval.naarInt(); y =yval.toInt(); startTime =millis(); if (isRotate =="r") { roteren(x); } else { driveInDirection(x,y); } rpiString=""; } //ENDIF} String getValue (String-gegevens, tekenscheidingsteken, int-index) {int gevonden =0; int strIndex[] ={0, -1 }; int maxIndex =data.length()-1; for(int i=0; i<=maxIndex &&gevonden<=index; i++){ if(data.charAt(i)==scheidingsteken || i==maxIndex){ gevonden++; strIndex[0] =strIndex[1]+1; strIndex[1] =(i ==maxIndex) ? ik+1 :ik; } } return gevonden>index ? data.substring(strIndex[0], strIndex[1]) :"";}

Arduino Sketch - MotorControlArduino

/* * Motorbesturingscode * * Deze klasse bevat code om de robot in elke richting te laten gaan * en rond het middelpunt te laten draaien. */void driveInDirection(float newX, float newY){ delay(20); zweven x =nieuweX; zwevend y =nieuwY; float theta =atan2(y,x); float mag =sqrt((x*x) + (y*y)); float vx =mag * cos(theta); float vy =mag * sin(theta); zweven w1 =-vx; zweven w2 =0,5 * vx - sqrt (3)/2 * vy; zweven w3 =0,5 * vx + sqrt (3)/2 * vy; // Haal de grootste w-waarde op float wSet [] ={w1, w2, w3}; float grootsteWaarde =0,0; for (int i =0; i <3; i++) {if(abs(wSet[i])> grootsteWaarde) {grootsteWaarde =abs(wSet[i]); } } float speedCoef =(float)147.0 / grootste waarde; w1 =w1 * speedCoef; w2 =w2 * speedCoef; w3 =w3 * speedCoef; als (x ==0 &&y ==0) { w1 =0; w2 =0; w3 =0; } Serieel.println(w1); Serieel.println(w2); Serieel.println(w3); w1 =beperking(w1, -150, 150); w2 =beperking (w2, -150, 150); w3 =beperking (w3, -150, 150); boolean w1_ccw =w1 <0 ? waar onwaar; boolean w2_ccw =w2 <0 ? waar onwaar; booleaans w3_ccw =w3 <0 ? waar onwaar; byte w1_speed =(byte) map(abs(w1), 0, 150, 0, 255); byte w2_speed =(byte) map(abs(w2), 0, 150, 0, 255); byte w3_speed =(byte) map(abs(w3), 0, 150, 0, 255); printMotorSpeed(w1_speed, 1); printMotorSpeed(w2_speed, 2); printMotorSpeed(w3_speed, 3); analogWrite (speedPinA, w1_speed);// Stelt snelheidsvariabele in via PWM analogWrite (speedPinB, w2_speed); analogWrite (speedPinC, w3_speed);// Stelt snelheidsvariabele in via 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 roteren (float milliseconden){ float w1 =255; vlotter w2 =255; vlotter w3 =255; boolean w1_ccw =w1 <0 ? waar onwaar; boolean w2_ccw =w2 <0 ? waar onwaar; booleaans w3_ccw =w3 <0 ? waar onwaar; byte w1_speed =(byte) map(abs(w1), 0, 150, 0, 255); byte w2_speed =(byte) map(abs(w2), 0, 150, 0, 255); byte w3_speed =(byte) map(abs(w3), 0, 150, 0, 255); printMotorSpeed(w1_speed, 1); printMotorSpeed(w2_speed, 2); printMotorSpeed(w3_speed, 3); analogWrite (speedPinA, w1_speed);// Stelt snelheidsvariabele in via PWM analogWrite (speedPinB, w2_speed); analogWrite (speedPinC, w3_speed);// Stelt snelheidsvariabele in via 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); vertraging (milliseconden); analogWrite (speedPinA, 0);// Stelt snelheidsvariabele in via PWM analogWrite (speedPinB, 0); analogWrite (speedPinC, 0);//Stel de snelheidsvariabele in via PWM }void printMotorSpeed ​​(byte motorSpeed, int motor) {Serial.print ("Motor"); Seriële.afdruk (motor); Serieel.print(":"); Serial.println (motorSnelheid); }

FaceTracking C++ voor Ci20C/C++

#include "opencv2/objdetect/objdetect.hpp"#include "opencv2/highgui/highgui.hpp"#include "opencv2/imgproc/imgproc.hpp"#include #include met namespace std;met namespace cv;CascadeClassifier face_cascade, eyes_cascade;String window_name ="Gezichtsdetectie";#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;}/** * Detecteert gezichten en tekent er een ellips omheen */void detectFaces(Mat frame) { std::vector faces; Mat frame_grijs; // Converteren naar grijsschaal cvtColor (frame, frame_gray, COLOR_BGR2GRAY); // Equalize histogram equalizeHist (frame_gray, frame_gray); // Detecteer gezichten face_cascade.detectMultiScale (frame_gray, faces, 1.1, 3, 0 | CASCADE_SCALE_IMAGE, Size (30,30)); // Herhaal alle vlakken voor (size_t i =0; i  ogen; // Probeer ogen te detecteren, in elk gezicht // eyes_cascade.detectMultiScale (gezicht, ogen, 1.1, 2, // 0 | CASCADE_SCALE_IMAGE, Grootte (50, 50)); // if(eyes.size()> 0) // Teken ellips rond gezicht ellips (frame, center, Size(faces[i].width/2, faces[i].height/2), 0, 0, 360 , Scalair ( 255, 0, 255 ), 4, 8, 0 ); if(center.x> frame.cols/3 &¢er.x =0) // pauze pauze; } retourneer 0;}

Schema's

Zie een betere beschrijving in het verhaalgedeelte