Robot amigable de seguimiento de personas omnidireccional

Código

• Arduino Sketch - Principal
• Arduino Sketch - MotorControl
• FaceTracking C ++ para Ci20

Arduino Sketch - Principal Arduino

 // Motor 1int dir1PinA =3; int dir2PinA =2; int speedPinA =9; // Debe ser un pin PWM para poder controlar la velocidad del motor // Motor 2int dir1PinB =4; int dir2PinB =5; int speedPinB =8; // Debe ser un pin PWM para poder controlar la velocidad del motor // Motor 3int dir1PinC =6; int dir2PinC =7; int speedPinC =10; // Debe ser un pin PWM para poder controlar la velocidad del motor int x =0; int y =0; int dominanteUltrasonic =0; bool moveMotor =false; int startTime =0; void setup () {Serial.begin (9600); pinMode (dir1PinA, SALIDA); pinMode (dir2PinA, SALIDA); pinMode (speedPinA, SALIDA); pinMode (dir1PinB, SALIDA); pinMode (dir2PinB, SALIDA); pinMode (speedPinB, SALIDA); pinMode (dir1PinC, SALIDA); pinMode (dir2PinC, SALIDA); pinMode (speedPinC, SALIDA); startTime =millis ();} String rpiString; void loop () {readDataFromRPi (); // Esto prueba si hemos recibido un valor del RPi en el último segundo. // Actúa eficazmente como búfer para que no siga deteniéndose y comenzando ya que el comando de movimiento no es continuo // También permite la detención si no llega nada desde el RPi // int elapsedTime =millis () - startTime; // if (elapsedTime> 1000) // {// x =0; // y =0; // dominanteUltrasonic =0; // moveMotor =false; // startTime =millis (); // //} // Envíe X e Y a los motores // if (moveMotor ==true &&(x! =0 &&y! =0)) // {//Serial.println("MovingMotor "); // driveInDirection (x, y); //} // if (x ==0 &&y ==0) // {//Serial.println("ZeroMMotor "); // driveInDirection (x, y); //} // Esto podría ser un sobrante de los ultrasonidos - vacila en eliminar delay (30);} void readDataFromRPi () {// Leer desde Rpi while (Serial.available ()) {delay (3); // demora para permitir que el búfer se llene if (Serial.available ()> 0) {char c =Serial.read (); // obtiene un byte del búfer serie rpiString + =c; // hace que la cadena sea readString if (c =='n') {break; }}} // ENDWHILE // Si se ha leído algo del RPi, póngalo en x, y &domniantUltrasonic if (rpiString.length ()> 0) {Serial.println (rpiString); // ver lo que se recibió String isRotate =getValue (rpiString, '', 0); Cadena xval =getValue (rpiString, '', 1); Cadena yval =getValue (rpiString, '', 2); x =xval.toInt (); y =yval.toInt (); startTime =millis (); if (isRotate =="r") {rotate (x); } else {driveInDirection (x, y); } rpiString =""; } // ENDIF} String getValue (datos de cadena, separador de caracteres, índice int) {int encontrado =0; int strIndex [] ={0, -1}; int maxIndex =data.length () - 1; for (int i =0; i <=maxIndex &&encontrado <=índice; i ++) {if (data.charAt (i) ==separator || i ==maxIndex) {encontrado ++; strIndex [0] =strIndex [1] +1; strIndex [1] =(i ==maxIndex)? i + 1:i; }} retorno encontrado> índice? data.substring (strIndex [0], strIndex [1]):"";} 

Arduino Sketch - MotorControl Arduino

 / * * Código de control del motor * * Esta clase incluirá código para hacer que el robot vaya en cualquier dirección * y gire alrededor del punto central. * / void driveInDirection (float newX, float newY) {delay (20); flotar x =newX; flotar y =newY; flotante theta =atan2 (y, x); flotante mag =sqrt ((x * x) + (y * y)); flotar vx =mag * cos (theta); flotar vy =mag * sin (theta); flotar w1 =-vx; flotar w2 =0.5 * vx - sqrt (3) / 2 * vy; flotar w3 =0.5 * vx + sqrt (3) / 2 * vy; // Obtener el valor w más grande float wSet [] ={w1, w2, w3}; float largerValue =0.0; for (int i =0; i <3; i ++) {if (abs (wSet [i])> mayorValor) {mayorValor =abs (wSet [i]); }} float speedCoef =(float) 147.0 / largerValue; w1 =w1 * speedCoef; w2 =w2 * speedCoef; w3 =w3 * speedCoef; si (x ==0 &&y ==0) {w1 =0; w2 =0; w3 =0; } Serial.println (w1); Serial.println (w2); Serial.println (w3); w1 =restringir (w1, -150, 150); w2 =restringir (w2, -150, 150); w3 =restringir (w3, -150, 150); booleano w1_ccw =w1 <0? verdadero Falso; booleano w2_ccw =w2 <0? verdadero Falso; booleano w3_ccw =w3 <0? verdadero Falso; byte w1_speed =(byte) mapa (abs (w1), 0, 150, 0, 255); byte w2_speed =(byte) mapa (abs (w2), 0, 150, 0, 255); byte w3_speed =(byte) mapa (abs (w3), 0, 150, 0, 255); printMotorSpeed ​​(w1_speed, 1); printMotorSpeed ​​(w2_speed, 2); printMotorSpeed ​​(w3_speed, 3); analogWrite (speedPinA, w1_speed); // Establece la variable de velocidad a través de PWM analogWrite (speedPinB, w2_speed); analogWrite (speedPinC, w3_speed); // Establece la variable de velocidad a través de PWM digitalWrite (dir1PinA,! w1_ccw); escritura digital (dir2PinA, w1_ccw); escritura digital (dir1PinB,! w2_ccw); escritura digital (dir2PinB, w2_ccw); escritura digital (dir1PinC, w3_ccw); digitalWrite (dir2PinC,! w3_ccw);} void rotate (float milisegundos) {float w1 =255; flotador w2 =255; flotador w3 =255; booleano w1_ccw =w1 <0? verdadero Falso; booleano w2_ccw =w2 <0? verdadero Falso; booleano w3_ccw =w3 <0? verdadero Falso; byte w1_speed =(byte) mapa (abs (w1), 0, 150, 0, 255); byte w2_speed =(byte) mapa (abs (w2), 0, 150, 0, 255); byte w3_speed =(byte) mapa (abs (w3), 0, 150, 0, 255); printMotorSpeed ​​(w1_speed, 1); printMotorSpeed ​​(w2_speed, 2); printMotorSpeed ​​(w3_speed, 3); analogWrite (speedPinA, w1_speed); // Establece la variable de velocidad a través de PWM analogWrite (speedPinB, w2_speed); analogWrite (speedPinC, w3_speed); // Establece la variable de velocidad a través de PWM digitalWrite (dir1PinA,! w1_ccw); escritura digital (dir2PinA, w1_ccw); escritura digital (dir1PinB,! w2_ccw); escritura digital (dir2PinB, w2_ccw); escritura digital (dir1PinC, w3_ccw); escritura digital (dir2PinC,! w3_ccw); retraso (milisegundos); analogWrite (speedPinA, 0); // Establece la variable de velocidad a través de PWM analogWrite (speedPinB, 0); analogWrite (speedPinC, 0); // Establece la variable de velocidad a través de PWM} void printMotorSpeed ​​(byte motorSpeed, int motor) {Serial.print ("Motor"); Serial.print (motor); Serial.print (":"); Serial.println (motorSpeed); } 

FaceTracking C ++ para Ci20 C / C ++

 #include "opencv2 / objdetect / objdetect.hpp" #include "opencv2 / highgui / highgui.hpp" #include "opencv2 / imgproc / imgproc.hpp" #include  #include  using namespace std; using 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;} / ** * Detecta rostros y dibuja una elipse alrededor de ellos * / void detectFaces (Mat frame) {std ::vector  faces; Mat frame_gray; // Convertir a escala de grises cvtColor (frame, frame_gray, COLOR_BGR2GRAY); // Ecualizar histograma equalizeHist (frame_gray, frame_gray); // Detectar caras face_cascade.detectMultiScale (frame_gray, faces, 1.1, 3, 0 | CASCADE_SCALE_IMAGE, Size (30,30)); // Itera sobre todas las caras para (size_t i =0; i  ojos; // Intenta detectar ojos, dentro de cada rostro // eyes_cascade.detectMultiScale (face, eyes, 1.1, 2, // 0 | CASCADE_SCALE_IMAGE, Size (50, 50)); // if (eyes.size ()> 0) // Dibuja una elipse alrededor de la elipse de la cara (frame, center, Size (faces [i] .width / 2, faces [i] .height / 2), 0, 0, 360 , Escalar (255, 0, 255), 4, 8, 0); if (center.x> frame.cols / 3 &¢er.x  =0) // pausa pausa; } return 0;} 

Esquemas

Ver una mejor descripción en la sección de historias