Thursday, 9 February 2017

mbed servo control system with OpenCV - Zhuo Fan Ye

Hi guys,

I'm Zhuo Fan Ye, doing my final year of B.Eng(H) in Electronics Eng. This project that I have been working on for the past 2 weeks involves getting servos linked to the mbed to be controlled by a vision system input, with the help of RTOS services in mbed.


Vision System Input

The vision system input is coded in python. With the help of a 3rd party library called OpenCV, and a working example on python. I was able to modify it to suit the needs of this project. The screenshot below (Figure 1) demonstrates the vision system input in action


Figure 1: face detection vision system input

Once the face has been detected (assuming PC has a camera or external camera), the functions in OpenCV is used to draw a rectangle around it for clarity, replacing the subsequent video frames so that verify our faces being detected by seeing the position of the rectangle there.

import cv2
import sys
import numpy as np
import serial

ser=serial.Serial(port='COM3',baudrate=9600,timeout=0)
ser.write('r')

#cascPath = sys.argv[1] if len(sys.argv) > 1 else '.'
cascPath = "C:/Anaconda/Scripts/myscripts/haarcascade_frontalface_default.xml"
#cascPath = sys.argv[1]
faceCascade = cv2.CascadeClassifier(cascPath)

#print(faceCascade)

video_capture = cv2.VideoCapture(0)

#print(video_capture)

while True:
    # Capture frame-by-frame

    ret, frame = video_capture.read()

    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    #print(gray)

    faces = faceCascade.detectMultiScale(
        gray,
        scaleFactor=1.1,
        minNeighbors=5,
        minSize=(30, 30),
        flags=cv2.cv.CV_HAAR_SCALE_IMAGE
    )

    #print(faces)
    # Draw a rectangle around the faces
    for (x, y, w, h) in faces:
        cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2)
        
###########################################ZF Edit###########################################
        a = str(x) + " " + str(y)
        ser.write(str(a))
        print(a)
###############################################################################################


    # Display the resulting frame
    cv2.imshow('Video', frame)





    k = cv2.waitKey(1) & 0xFF
    if k == 27:
        break

# When everything is done, release the capture
video_capture.release()
cv2.destroyAllWindows()

The x and y coordinates of a point of the rectangle (that is following the face) is used as reference point (note that this is not the centrepoint of the face! That x and y has to be readjusted later) for the servos to respond to. The coordinates are concatenated as a string are sent via serial input which is then received by the mbed.


Figure 2: x and y coordinates

The x and y coordinates that are received via the serial (type string and separated by delimiter "x") is to be split back into two variables again - one to control each servo which will move in the x axis and y axis individually.

The program below is created as a test run to split strings based on identifying a delimiter (which will be used to split the x and y coordinates further on

#include
#include
#include
#include
using namespace std;
/* run this program using the console pauser or add your own getch, system("pause") or input loop */

int main(int argc, char** argv) {


std::string s = "1233x446";
std::string delimiter = "x";


cout << "Sample string that is concantenated: " << s << endl;

size_t pos = 0;
std::string token;
while ((pos = s.find(delimiter)) != std::string::npos) {
    token = s.substr(0, pos);
    std::cout << token << std::endl;
    s.erase(0, pos + delimiter.length());
}
std::cout << s << std::endl;


return 0;
}



Figure 3

The serial port data (string) is to be splitted like shown in the example program in Figure 3 so the x and y coordinates can be seperated

Servo control using x and y serial input
Tinkering around the sample code provided in the mbed cookbook:

#include "mbed.h"
#include "Servo.h"
 
Servo myservo(p21);
Serial pc(USBTX, USBRX);
 
int main() {
    printf("Servo Calibration Controls:\n");
    printf("1,2,3 - Position Servo (full left, middle, full right)\n");
    printf("4,5 - Decrease or Increase range\n");
 
    float range = 0.0005;
    float position = 0.5;
    
    while(1) {                   
        switch(pc.getc()) {
            case '1': position = 0.0break;
            case '2': position = 0.5break;
            case '3': position = 1.0break;
            case '4': range += 0.0001break; 
            case '5': range -= 0.0001break; 
        }
        printf("position = %.1f, range = +/-%0.4f\n", position, range);
        myservo.calibrate(range, 45.0); 
        myservo = position;
    }
}
The serial input for x and y has a limit of 640 x 400. And the range of myservo goes from 0 to 1. With that in mind, the next step would be to have two position variables scale the float position to input_x/640 for the x coordinate and input_y/400 for the y coordinate. And with input_x and input_y being obtained from the spitted string that is received through the serial port. 
Here is a snippet of a small part of the code (still incomplete) that is to be completed and implemented to control the servo
string k = pc.getc();  //TODO: Convert k to float
float x,position;
    while(1)      {                           x = (float(k)/float(580));  //580 because x only ranges from 0 to 580           position = x;         myservo = position;         //updates servo to the new position         wait(0.2);              }





No comments:

Post a Comment