Final Report

Terminatorby Ju Zong

April 22, 2014

University of Florida Department of Electrical and Computer Engineering

EEL 5666Instructors: A. Antonio Arroyo, Eric M. Schwartz

TAs: Josh Weaver, Andy Gray, Nick Cox, Daniel Frank

Table of Contents

Abstract ….................................................................................................................................................2

Executive Summary …..............................................................................................................................2

Introduction …...........................................................................................................................................2

Integrated System …..................................................................................................................................3

Mobile Platform …....................................................................................................................................3

Actuation …...............................................................................................................................................4

Sensors …..................................................................................................................................................4

Behaviors …..............................................................................................................................................5

Experimental Layout and Results ….........................................................................................................6

Conclusion …............................................................................................................................................7

Documentation …......................................................................................................................................8

Appendices …............................................................................................................................................8

1

Abstract Robot has been a perfect candidate for exploring the dangerous and unknown places for a long time. NASA sent voyagers to mars tens of years ago and they are still transmitting data back to earth. Space radiation, high temperature, lack of oxygen, existence of toxic substances etc., prevent people from doing jobs in extreme conditions, therefore the need to have a robot is urgent. This paper describes an autonomous robot in details, including various hardware and software parts.

Executive SummaryTerminator is an autonomous robot that can randomly move in a room. While moving, it can avoid obstacles and use the camera in front to search for a target. After the target is found, it will use the hammer-like piece of wood to hit the target. Then the whole operation terminates and is reset to the initial stage.

The robot is actuated by two motors. The hammer-like piece of wood is driven by a servo. Three IR sensors are used to achieve the obstacle avoidance and also help the robot approach the target. One IP camera can send the image to the laptop where the images are processed. Once the images processing is finished, the laptop will send command though a pair of Xbee modules. One is on the laptop side, and the other is on the robot. The arduino board on the robot will then read the command and direct the robot based on the command. All of the above components are powered by a Lipo battery attached below its platform.

There are two programs running at the same time. One is on the Terminator. It mainly controls how the robot should behave based on the command sent from the laptop. The other is on the laptop. It is a program written with opencv functions. The program can find a blue ball in the image if the ball is there and correspondingly send the left, right, and middle command to the robot, directing the robot to approach the ball. The program for image processing is very sensitive to the environment (for example, light) and can be inaccurate since the robot is constantly moving.

The whole operation is designed as a finite state machine. When powered on, Terminator enters the STALL stage, and listens to the command send by the computer. Then the computer program runs, sending a start command to Terminator and it begins to move randomly, entering the ROAM stage.When moving, it sends the images to the computer through the IP camera and if the target appears in the images judged by the program, the computer will send command to direct the robot to approach the target in the images. Now the robot is in the APPROACH stage. When the robot is close to the target but not close enough to hit it with the wood, the robot enters NEAR stage, and will use IR sensors to accurately locate the target. Finally, the robot enters the FIND stage and begin to hit the target. After all the operation is done, Terminator goes back to the STALL stage and wait for new command.

IntroductionAs the technology advances, people are becoming more and more rely on machines to do jobs that are too difficult or unsuitable for humans to do. For example, in order to search for specific targets in a dangerous environment, we can build and program a robot and let the robot do the searching. We can

2

view the images sent from the robot and control it from the far end. This paper describes an autonomous robot that is built for the project.

The paper will present the whole construction of the robot, including the platform, actuators, sensors and the software that controls different parts.

Integrated SystemThe control of motors and servo is through the motor shield. IR sensors, and Xbee1 are connected to the Arduino directly. IP camera is on the platform, but is not connected to Arduino. It sends images to the router, and the laptop can access the images though the router. After the image processing is done, the laptop sends out command through the Xbee2 to Xbee1.

The whole block diagram is shown in Figure 1.

Figure 1: Integrated System

Mobile PlatformThe platform is designed to be two-level. The bottom level almost accommodate everything, except the servo that controls the wood. The purpose of the top level is to reside the hammer-like piece of wood, and offer more room for future components that may be added.

3

Arduino

IR sensors

IP camera

Motor shield

motors

Weapon servoXbee1

Xbee2

Laptop

Router

ActuationThe robot is driven by two motors. In order to properly drive the whole robot which can be very heavy, the motors need to have a large torque. Since image processing is very sensitive, if the speed of the robot is too high, the software program has a higher chance of not being able to recognize the target. So we decided to choose a low speed motor.

Finally, we chose the 12v, 55rpm, JGB37-371 Permanent magnet DC Geared motor. To supply enough power to the motor, we also purchased the 3-cell 11.1V Lipo battery. This battery supplies power to all other components as well.

To control the motors, we use a motor shield and download the library necessary to write motor-control codes. The library's name is Afmotors. The motor shield is shown in Figure 4. The code for the motors can be found in the Appendices[1]. More information can be found in Documentation.

Figure 2: motor shield

SensorsThree IR sensors are located in the left, middle and right side of the robot, taking care of the obstacles in the corresponding direction. Figure 3 shows the IR sensor. The red line is connected to Vcc; the black line is connected to GND; the yellow line is connected to analog pin of Arduino. When there is an obstacle, the digital pin will read a higher value from the yellow wire. The code for IR sensors can be found in Appendices[1]. More information can be found in Documentation.

The vision sensor is the DCS-930L IP camera (Figure 4). It can take 640x360 images. The power supply for it comes from a voltage regulator that converts the 11.1V Lipo battery to 5V power source. The code for open IP camera and take images from web can be found in Appendices[2]. More information can be found in Documentation.

4

Figure 3: IR sensor Figure 4: IP camera

For the communication between the robot and the laptop, we chose the Xbee module. One Xbee module has to be in serial communication with the Arduino, so the Arduino can get signal from the other Xbee. The Xbee Series2 is shown in Figure 5, and more information can be found in Documentation.

Figure 5: Xbee Series 2

BehaviorsThe laptop program should run to ensure the proper functioning of the robot. The program will send 'l', 'm', 'r' when the robot sees the target; it will send nothing otherwise.

The whole operation algorithm is designed as a finite state machine. When powered on, Terminator

5

enters the STALL stage, and listens to the command send by the computer. Then the computer program runs, sending a start command to Terminator and it begins to move randomly, entering the ROAM stage. When moving, it sends the images to the computer through the IP camera and if the target appears in the images judged by the program, the computer will send command to direct the robot to approach the target in the images. Now the robot is in the APPROACH stage. When the robot is close to the target but not close enough to hit it with the wood, the robot enters NEAR stage, and will use IR sensors to accurately locate the target. Finally, the robot enters the FIND stage and begin to hit the target. After all the operation is done, Terminator goes back to the STALL stage and wait for new command.

The whole FSM is shown in Figure 6.

Figure 6: Behavior Algorithm (FSM)

Experimental Layout and ResultsThe core part for the software is to detect a circle with certain color. Below is an image for the test of this program.

6

STALL

'y' received?

ROAM

Command:'r' 'l' 'm' received?

APPROACH

NEAR

Is 'l' 'm' 'r' still available?

Do the IR sense the target?

FIND

no

yes

no

yes

yes

no

yes

no

Figure 7: test of software

The software then decide whether it should turn left or right based on the location of the ball relative to the frame. The robot then turn left or right correspondingly.

ConclusionIn this project, I have learned much knowledge about mechanical engineering, electrical engineering and computer science. I made the robot platform using solidworks; I chose the motor that has enough torque to drive the robot; I learned how to program the Arduino; I learned how to establish RF communication between two Xbee modules; I learned how to program c++ with opencv and serial port library; I learned how to configure an IP camera and take image from web in a c++ program. These are all valuable experiences.

In the end, I completed most of the functions in my robot proposal. It can walk randomly and can find then attack a target using the camera. There are also rooms to improve. Firstly, the program running on the laptop is quite sensitive to the light intensity around. If the background is dark, then dark color target is not easy to identify. Writing a more descent program may be able to improve the sensitivity. Secondly, my robot only recognizes one specific target. In future work, identifying more targets can be accomplished by expanding the FSM of the behavior algorithm.

DocumentationArduino mega 2560: http://arduino.cc/en/Main/arduinoBoardMega2560IR sensor: http://www.pololu.com/product/136DCS-930L IP camera: http://www.dlink.com/us/en/home-solutions/view/network-cameras/dcs-930l-wireless-n-network-cameraMotor shield: http://www.adafruit.com/products/81Xbee: https://www.sparkfun.com/products/10414

Appendices

7

[1] Arduino code#include <AFMotor.h>#include <Servo.h>

AF_DCMotor motor1(3);AF_DCMotor motor2(2);int left = 15;int middle = 14;int right = 13;char cmd = 'c';int num = 0;Servo myservo;int pos = 0;//below is the state machineint state = 0;int nextState = 0;int noSignalCount = 0;int adjustServo = 0;# define STALL 0# define ROAM 1# define APPROACH 2# define NEARTARGET 3# define FIND 4# define ATTACK 5

void blinkLED(){ digitalWrite(30,HIGH);}

int targetIsNear(){ int val_l = analogRead(left); int val_m = analogRead(middle); int val_r = analogRead(right); if(val_m > 700 /*|| val_l>600 || val_r>600*/){ stopMoving(); return 1; } else if(val_m>200 /*&& val_m<600*/){ goStraight(50); stopMoving(); return 0; }// else if(val_l>200 /*&& val_l<600*/){// turnLeft(50);

8

// stopMoving();// return 0;// }// else if(val_r>200 /*&& val_r<600*/){// turnRight(50);// stopMoving();// return 0;// } else if(val_l<100 && val_m<100 && val_r<100){ return -1; } else{ turnRight(0); return 0; } }void attack(int t){ for(int i=0; i<t; i++){ for(pos = 90; pos < 180; pos += 1) { myservo.write(pos); delay(15); } for(pos = 180; pos>=90; pos-=1) { myservo.write(pos); delay(15); } }}

void clearBuffer(){ int temp = Serial1.available(); for(int i=0; i<temp; i++) Serial1.read();}

void resetServo(){ int p = myservo.read(); if(p > 95){ while(p>95){ myservo.write(p); p=p-1; delay(15);

9

} } else if(p<85){ while(p<85){ myservo.write(p); p=p+1; delay(15); } }}void setup(){ Serial1.begin(9600); pinMode(left, INPUT); pinMode(middle, INPUT); pinMode(right, INPUT); pinMode(30, OUTPUT); pinMode(28, OUTPUT); motor1.setSpeed(200); motor1.run(RELEASE); motor2.setSpeed(200); motor2.run(RELEASE); myservo.attach(32); }

void turnRight(int t){ motor1.run(FORWARD); motor2.run(BACKWARD); motor1.setSpeed(200); motor2.setSpeed(200); delay(t);}

void turnLeft(int t){ motor2.run(FORWARD); motor1.run(BACKWARD); motor1.setSpeed(200); motor2.setSpeed(200); delay(t);}

void goStraight(int t){ motor1.run(FORWARD); motor2.run(FORWARD); motor1.setSpeed(200); motor2.setSpeed(191);

10

delay(t);}

void goBackward(int t){ motor1.run(BACKWARD); motor2.run(BACKWARD); motor1.setSpeed(200); motor2.setSpeed(200); delay(t);}

void stopMoving(){ //motor1.run(FORWARD); //motor2.run(FORWARD); motor1.run(RELEASE); motor2.run(RELEASE);}

void randomWalk(){ int val_l = analogRead(left); int val_m = analogRead(middle); int val_r = analogRead(right); if(val_l<400 && val_m<400 && val_r<400) goStraight(0); else if(val_l>400 && val_m<400 && val_r<400) turnRight(0); else if(val_l<400 && val_m>400 && val_r<400){ turnRight(0); } else if(val_l>400 && val_m>400 && val_r<400){ turnRight(0); } else if(val_l<400 && val_m<400 && val_r>400) turnLeft(0); else if(val_l<400 && val_m>400 && val_r>400){ turnLeft(0); } else if(val_l>400 && val_m>400 && val_r>400){ goBackward(1000); turnRight(500); } else stopMoving();}

void approachTarget(){ int num = Serial1.available(); cmd = 'c';

11

cmd = char (Serial1.read()); switch(cmd){ case 'm': {goStraight(300); stopMoving();break;} case 'l': {turnLeft(50);stopMoving(); break;} case 'r': {turnRight(50); stopMoving();break;} case 's': {/*blinkLED()*/; attack(3);adjustServo=0;clearBuffer(); nextState=STALL;break;} default: {stopMoving(); break;} } if(num>2){ for(int i=0; i<num-1; i++) int damp = Serial1.read(); } num = 0;}void loop(){// delay(5000);// resetServo();// attack(3); //randomWalk();// switch(state){ case STALL: { stopMoving(); if(adjustServo==0){ resetServo(); adjustServo = 1; } if(Serial1.available()>0 && char(Serial1.read())=='y'){ nextState = ROAM; } break; } case ROAM: { randomWalk(); if(Serial1.available()>0){ char temp = char(Serial1.peek()); if(temp=='l' || temp=='r' || temp=='m'){ nextState = APPROACH; } } break; } case APPROACH: { if(Serial1.available()>0){ approachTarget();

12

noSignalCount = 0; } else if(noSignalCount<10){ delay(200); noSignalCount ++; } else if (noSignalCount>= 10){ nextState = NEARTARGET; } stopMoving(); break; } case NEARTARGET: { //targetIsNear = -1,0,1 for far away, near but not find, find int temp = targetIsNear(); if(temp == -1){ nextState = ROAM; } else if(temp == 0){ nextState = NEARTARGET; } else if(temp == 1){ nextState = FIND; } break; } case FIND: { //blinkLED(); attack(3); adjustServo=0; nextState=STALL; break; } default: { nextState = STALL; break; } } state = nextState;}

[2] C++ program#include <stdlib.h>#include <iostream>#include <stdio.h>

13

#include <math.h>#include <deque>#include "opencv/cv.h"#include "opencv/highgui.h"#include "opencv/cxcore.h"#include <SerialStream.h>

using namespace std;using namespace cv;using namespace LibSerial ;

CvSeq* getCirclesInImage(IplImage*, CvMemStorage*, IplImage*);float eucdist(CvPoint, CvPoint);void drawCircleAndLabel(IplImage*, float*, const char*, SerialStream&);bool circlesBeHomies(float*, float*);

const int MIN_IDENT = 50;const int MAX_RAD_DIFF = 10;const int HISTORY_SIZE = 5;const int X_THRESH = 15;const int Y_THRESH = 15;const int R_THRESH = 20;const int MATCHES_THRESH = 3;const int HUE_BINS = 32;

int main(int argc, char *argv[]) { const char* const SERIAL_PORT_DEVICE = "/dev/ttyUSB0" ; SerialStream serial_port ; serial_port.Open( SERIAL_PORT_DEVICE ) ; if ( ! serial_port.good() ) { std::cerr << "Error: Could not open serial port " << SERIAL_PORT_DEVICE << std::endl ; exit(1) ; } // // Set the baud rate of the serial port. // serial_port.SetBaudRate( SerialStreamBuf::BAUD_9600 ) ; if ( ! serial_port.good() ) { std::cerr << "Error: Could not set the baud rate." << std::endl ; exit(1) ; } // // Set the number of data bits. // serial_port.SetCharSize( SerialStreamBuf::CHAR_SIZE_8 ) ; if ( ! serial_port.good() ) { std::cerr << "Error: Could not set the character size." << std::endl ; exit(1) ; } //

14

// Disable parity. // serial_port.SetParity( SerialStreamBuf::PARITY_NONE ) ; if ( ! serial_port.good() ) { std::cerr << "Error: Could not disable the parity." << std::endl ; exit(1) ; } // // Set the number of stop bits. // serial_port.SetNumOfStopBits( 1 ) ; if ( ! serial_port.good() ) { std::cerr << "Error: Could not set the number of stop bits." << std::endl ; exit(1) ; } // // Turn on hardware flow control. // serial_port.SetFlowControl( SerialStreamBuf::FLOW_CONTROL_NONE ) ; if ( ! serial_port.good() ) { std::cerr << "Error: Could not use hardware flow control." << std::endl ; exit(1) ; }

CvCapture *capture = 0; //The camera IplImage* frame = 0; //The images you bring out of the camera //Open the camera capture = cvCaptureFromFile("http://admin:rzc36698@192.168.1.3/video.cgi?.mjpeg"); //capture = cvCaptureFromCAM(0); if (!capture ) { printf("Could not connect to camera\n" ); return 1; }

frame = cvQueryFrame( capture ); //Create two output windows //cvNamedWindow( "raw_video", CV_WINDOW_AUTOSIZE ); cvNamedWindow( "processed_video", CV_WINDOW_AUTOSIZE );

//Used as storage element for Hough circles CvMemStorage* storage = cvCreateMemStorage(0);

// Grayscale image IplImage* grayscaleImg = cvCreateImage(cvSize(640, 480), 8/*depth*/, 1/*channels*/);

// CvPoint track1 = {-1, -1};// CvPoint track2 = {-1, -1};

15

// float rad1 = -1;// float rad2 = -1; deque<CvSeq*> samples; int key = 0; //take 1 frame every count frames //int count = 0; char begin = 'c'; cout<<"Begin?"<<endl; cin>>begin; serial_port.write(&begin, 1); while(key != 27 /*escape key to quit*/ ) { //Query for the next frame frame = cvQueryFrame( capture );// count ++;// if(count != 2)// continue;// else{// count = 0;// } if( !frame ) break;

deque<CvSeq*> stableCircles; //show the raw image in one of the windows //cvShowImage( "raw_video", frame ); CvSeq* circles = getCirclesInImage(frame, storage, grayscaleImg);

//find the largest circle int index = -1; int max = 0; for(int i=0; i< circles->total; i++){ float* p = (float*)cvGetSeqElem( circles, i ); if(p[2] > max){ max = p[2]; index = i; } }

//Iterate through the list of circles found by cvHoughCircles() for(int i = 0; i < 1&& index != -1; i++ ) { int matches = 0; float* p = (float*)cvGetSeqElem( circles, index ); float x = p[0]; float y = p[1]; float r = p[2]; if (x-r < 0 || y-r < 0 || x+r >= frame->width || y+r >= frame->height) { continue; } for (int j = 0; j < samples.size(); j++) { CvSeq* oldSample = samples[j]; for (int k = 0; k < oldSample->total; k++) { float* p2 = (float*)cvGetSeqElem( oldSample, k ); if (circlesBeHomies(p, p2)) { matches++; break; }

16

} } if (matches > MATCHES_THRESH) { cvSetImageROI(frame, cvRect(x-r, y-r, 2*r, 2*r)); IplImage* copy = cvCreateImage(cvSize(2*r, 2*r), frame->depth, 3); cvCvtColor(frame, copy, CV_BGR2HSV); IplImage* hue = cvCreateImage(cvGetSize(copy), copy->depth, 1); cvCvtPixToPlane(copy, hue, 0, 0, 0); int hsize[] = {HUE_BINS}; float hrange[] = {0,180}; float* range[] = {hrange}; IplImage* hueArray[] = {hue};// int channel[] = {0}; CvHistogram* hist = cvCreateHist(1, hsize, CV_HIST_ARRAY, range, 1); cvCalcHist(hueArray, hist, 0, 0); cvNormalizeHist(hist, 1.0); int highestBinSeen = -1; float maxVal = -1; for (int b=0; b<HUE_BINS; b++) { float binVal = cvQueryHistValue_1D(hist, b); if (binVal > maxVal) { maxVal = binVal; highestBinSeen = b; } } cvResetImageROI(frame); const char *color; switch(highestBinSeen) { case 2: case 3: case 4: color = "orange"; break; case 5: case 6: case 7: case 8: color = "yellow"; break; case 9: case 10: case 11: case 12: case 13: case 14: case 15: case 16: color = "green"; break; case 17: case 18: case 19: case 20: case 21: case 22: case 23: color = "blue"; break; case 24: case 25: case 26: case 27: case 28: color = "purple"; break; default: color = "red"; } char label[64]; sprintf(label, "color: %s", color); //if(label[7] == 'b'){ drawCircleAndLabel(frame, p, label,serial_port); //} }

17

}// if(circles->total == 0){// char result = 'n';// std::cout<<"Not found"<<std::endl;// serial_port.write(&result,1);// } samples.push_back(circles); if (samples.size() > HISTORY_SIZE) { samples.pop_front(); } cvShowImage( "processed_video", frame);

//Get the last key that's been pressed for input key = cvWaitKey( 1 ); }}

CvSeq* getCirclesInImage(IplImage* frame, CvMemStorage* storage, IplImage* grayscaleImg) { // houghification // Convert to a single-channel, grayspace image cvCvtColor(frame, grayscaleImg, CV_BGR2GRAY);

// Gaussian filter for less noise cvSmooth(grayscaleImg, grayscaleImg, CV_GAUSSIAN, 7, 9 );

//Detect the circles in the image CvSeq* circles = cvHoughCircles(grayscaleImg, storage, CV_HOUGH_GRADIENT, 2, grayscaleImg->height/4, 200, 100 ); return circles;}

float eucdist(CvPoint c1, CvPoint c2) { float d = sqrt(pow((float)c1.x - c2.x, 2) + pow((float)c1.y - c2.y, 2)); return d;}

void drawCircleAndLabel(IplImage* frame, float* p, const char* label, SerialStream& serial_port) { //Draw the circle on the original image //There's lots of drawing commands you can use! cout<<"the center is ("<<p[0]<<", "<<p[1]<<")"<<endl; if(p[0] < 220){

char result = 'l'; std::cout<<"should turn left"<<std::endl; serial_port.write(&result,1);

} else if(p[0] > 420){

char result = 'r'; std::cout<<"should turn right"<<std::endl;

18

serial_port.write(&result,1); } else{

char result = 'c'; if(p[2] > 200){

result = 's'; cout<<"object found, should stop"<<endl;

} else{

result = 'm'; std::cout<<"should go straight"<<std::endl;

} serial_port.write(&result,1);

}

CvFont font; cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1, 1, 0.0, 1, 8); cvCircle( frame, cvPoint(cvRound(p[0]),cvRound(p[1])), cvRound(p[2]), CV_RGB(255,0,0), 3, 8, 0 ); cvPutText( frame, label, cvPoint(cvRound(p[0]),cvRound(p[1])), &font, CV_RGB(255,0,0) );}

bool circlesBeHomies(float* c1, float* c2) { return (abs(c1[0]-c2[0]) < X_THRESH) && (abs(c1[1]-c2[1]) < Y_THRESH) && (abs(c1[2]-c2[2]) < R_THRESH);}

19