Final Report Shoe Fetish

Transcription

1 Final Report Shoe Fetish Jing Zou University of Florida Department of Electrical and Computer Engineering EEL 5666 IMDL Instructors: A. Antonio Arroyo, Eric M. Schwartz TAs: Josh Weaver, Andy Gray, Devin Hughes

2 Contents Abstract... 3 Executive Summary... 3 Introduction... 3 Integrated System... 4 Mobile Platform... 4 Actuation System... 6 Sensors... 7 Behaviors... 7 System... 8 Sensors and their function... 错误! 未定义书签 Reference... 错误! 未定义书签

3 Abstract Shoe Fetish is a cleaning robot. It can search for shoes with specific colors on them, pick them up and relocate them on the shoes rack. And then it would start anther working circle and looking for another shoe, etc. Shoe Fetish could perform obstacle avoidance while searching for a shoe. We can interrupt its searching state and conduct remote control on the robot s moving direction, start or stop from the laptop. Shoe Fetish has potential in the application of room clean together with remote control entertainment. Executive Summary Shoe Fetish is an autonomous robot with the main function is to arrange the shoe in the room so as to keep the room in order. All the detection work is done by computer vision. Here the fake shoe I used is a box with a special color and the fake shoes rack is a colored paper. First, the robot utilizes a random route strategy to search the shoe over a restricted area, and with the IR sensors fixed in the front edge, it could perform obstacle avoidance during this searching mode. An IPhone is mounted on the robot to act as an IP camera. Images captured by the camera are sent back to the laptop through wifi. Then image processing is conducted on the laptop through OpenCV, achieving the goal of object detection using color recognition. After finding the object, first 'the shoe, the robot will move towards the detected object using camera to navigate its motion. After the object reaches the designed destination zone in the camera, the robot then make the judgment that the object is in the desire position for the next operation. Here the robot will pick up the shoe with the special scratch designed for grabbing the shoe. Afterward, the robot switches the working state, then searches for the shoes rack and reaches the shoes rack with the same method used for searching shoe. Upon finishing the find-locate process, the robot would lay down the shoe on the shoes rack. Till now, one working circle is done. Another working circle consists of the same procedures will starts, etc. The working state of the robot is presented on the LCD screen mounted at the top layer of the robot. And we can get the information about what the robot is currently engaged in through reading the words on the LCD. Apart from that, we can also interrupt its searching state and conduct remote control on the robot from the laptop, asking the robot to moving in the designated direction, stop moving and start moving. Introduction Many people do not like to pick up their shoes and put them on the shoes rack after they take off the shoes. Obviously this is a simply but not pleasant thing to do. I have the same issue with this assignment, and it motivates me to build an autonomous robot that could do this stuff for us. I use some tracking techniques from [1] and [2]. The objective of this project is to construct an autonomous robot which combines the function of arrange the shoes and remote control. The rest of this report is made of integrated system, mobile platform, actuation, sensors, behaviors,

4 experimental layout and results, conclusion, documentation and appendices. They give descriptions about the components of the robot system, their working performance and discussions. Integrated System Shoe Fetish is using an Arduino mega 2560 R3 microcontroller board as its controller. It is based on the ATmega2560. It has 54 digital input / output pins (of which 14 can be used as PWM outputs), 16 analog inputs, 4 UARTs (hardware serial ports), a 16 MHz crystal oscillator, a USB connection, a power jack, an ICSP header, and a reset button. Three IR rangefinders are utilized to fed directly to the Arduino Board for obstacle avoidance. An IPhone used as an IP camera is integrated into the control system through taking video of the environment in front of the robot and transmitted the video to a laptop over WiFi for processing and then sending the resultant command signals to the Arduino Board via two Xbees. The video processing is done on a laptop since the processing capability required is beyond the Arduino Board. The Arduino Board is in charge of platform movement and actuators control. System Configuration IR Rangefinder Motor Controller DC Motor Obstacle Detection Embed Board Atmega 2560 IP camera LCD Platform Image Router Laptop Inquiry Message Robot arm servos shoe Distance Xbee Resultant command Xbee The system configuration Mobile Platform To successfully find out, pick up and put the shoes on the shoes rack, the mechanical part should be able to first carry out the searching task. A mobile platform is designed as the figure:

5 The two front wheels are driven by two motors. The motors can drive the two wheels at different or the same speed, which accomplish the moving speed control and direction control of the platform.

6 Actuation System The actuation system is driven by the following actuators: Two 75:1 Metal Gearmotor 25Dx54L mm: gearmotor used to drive the robot. Two motors total each directly tied to the tires. Two HKSCM9-6 Singlechip Digital Micro Servo (6V): this small torque servo is used to drive the two scratches of the robot hand, giving the close and open motion. Corona DS558HV Digital Metal Gear Servo 14kg/ 58g/ 0.18 Sec: this medium torque servo is used to drive the arm of the robot arm, accomplish pull up and lay down motion. Robot Hand Structure: used for pick up the shoe.

7 Sensors IR Rangefinder: three IR are mounted on the front edge of the platform to perform obstacle detection in front of the platform and distance measurement. IP camera: mounted on the platform to accomplish the task of shoe and shoes rack detection and tracking. Behaviors

8 Robot is initially set to seek out for shoe and identify them as possible targets. While the robot is searching its objectives, it can perform obstacle avoidance. Once an object has been located, the robot must proceed to move towards its target, while adjusting its trajectory to compensate for any deviations. And then finish the pick up, relocate and lay down motion. Functions to fulfill those operations are listed below: Search: The robot is taking a random path strategy to search for the objective. First move forward for a random distance and then randomly decided the rotate direction and rotate angle of the platform. Pick up: To pick up the shoe, the robot is first opens its scratches. Then it pulls down the arm of its robot hand. After that, close its scratches, and finally pulls up its arm. Lay down: To lay down the shoe on the shoes rack, the robot first pulls down the arm of its robot hand. After that, it opens its scratches to let off the shoe on the shoes rack. Finally, the robot pulls up its arm. Obstacle avoidance: Obstacle avoidance function decides the motion of the platform by the synthesis of values coming from all the three IR rangefinders. If only the value from right side IR gets bigger than 30, the robot would first move backward for 500ms, then rotate left for 500ms, then continuous its original move. If only the value from left side IR gets bigger than 300, the robot would first move backward for 500ms, then rotate right for 500ms, then continuous its original move. If the value from the IR in the middle gets bigger than 300, the robot would first move backward for 500ms, then randomly rotate left or right for 500ms, then continuous its original move. If both the values from right side IR and left side IR get bigger than 300, the robot would first move backward for 500ms, then randomly rotate left or right for 500ms, then continuous its original move. Object tracking In order to get a good object tracking result, the robot is using the coordinates of the object in the camera coordinate system to get the deviation between the destination and real position of the object. Then coordinate system of the camera is shown below: X Y

9 For shoe tracking, and are the desired position. and are the coordinates of the center point of the shoe. and give the acceptable square area with as the center, where the object is considered ready for next operation. If, the platform would turn left. If, the platform would turn right. If, the robot will move forward if, and the robot will move backward if. The shoes rack tracking has the similar principle as shoe tracking. Experimental Layout and Results System To get a reliable obstacle avoidance result, we need to calibrate those IR rangefinders and figure about a general relationship between the distance and the analog values from IR rangefinders to the Arduino Board. The calibration experiment is carried out as follows: And the calibration result is: Object Distance Analog Value 3cm 506 6cm 422 9cm cm 238 The robot is working together with my laptop, when OpenCV gets running, the Xbee connected on the laptop starts to send out resultant command the other Xbee connected with the Arduino Board. And it is critical to know the performance of image processing in OpenCV and if correct commands have been sent out. The image processing procedure is: (1) read the images from the IP camera; (2) present the images on the laptop screen and transform them into an HSV images; (3) for the pixels that have the HSV value in the designated HSV value range, mark them with white, and for the pixels that have the HSV value out of the designated HSV value range, mark

10 them with black and calculate the center point coordinates of the selected color; (4) show the binary images on a new window, make judgment about which command should be sent and print the command on the command window. Processing results are presented below: Conclusion Robot design and construction is a completely new area for me. I struggled from the beginning to the very end. During this study and application process, I am forced to learn many new things, both software and hardware, very fast in order to keep in pace. Although having experienced many hard periods for this lab course, I still feel really good at the end of this semester because I did feel I learn pretty much from it. This practical course is really different from other theoretical lectures. You are free to come up with your own ideas and try your best to accomplish them. There are many fallbacks as well. I burned one Arduino Board just two days before the Demo day and it made everything like a whole mess. Besides, as I went deeper into my robot design and construction process, more ideas comes up. Also I realized that there are many things I can do to make my robot a better one. Some ideas could still be applied on the robot and make it better, while it is impossible for many others thoughts. This process is always like so, full of excitation and regret. However, I do obtain a lot, and I know next time when I am going to build a robot, I could make a more satisfactory one. Documents [1] Michael Boyco, The Librarian, Fall 2010 IMDL report. [2] OpenCV Tutorial C++,

11 [3] Arduino Mega 2560: [4] Tracking a ball and rotating camera with OpenCV and Arduino: [5] Pololu Dual MC33926 Motor Driver Shield User s Guide: [6] Pololu Dual MC33926 Motor Driver Shield Library Code: [7] LiquidCrystal - "Hello World!": Appendices OpenCV code for color detection, tracking and remote control #include "stdafx.h" #include "opencv\cv.h" #include "opencv\highgui.h" #include <iostream> #include <iostream> #include<opencv/cvaux.h> #include<opencv/cxcore.h> #include<stdio.h> #include<stdlib.h> // Need to include this for serial port communication #include <Windows.h> using namespace cv; using namespace std; IplImage* imgtracking1; IplImage* imgtracking2; IplImage* imgtracking3; int lastx1 = -1; int lasty1 = -1; int lastx2 = -1; int lasty2 = -1; int lastx3 = -1; int lasty3 = -1; //set up the destination x,y coordinates for 3 object int X1=237; int Y1=210; int X2=205;

12 int Y2=205; int X3=235; int Y3=218; int SM = 0; int ST = 0; //function to rotate the image IplImage *rotateimage(const IplImage *src, int angledegrees) IplImage *imagerotated = cvcloneimage(src); if(angledegrees!=0) CvMat* rot_mat = cvcreatemat(2,3,cv_32fc1); // Compute rotation matrix CvPoint2D32f center = cvpoint2d32f( cvgetsize(imagerotated).width/2, cvgetsize(imagerotated).height/2 ); cv2drotationmatrix( center, angledegrees, 1, rot_mat ); // Do the transformation cvwarpaffine( src, imagerotated, rot_mat ); return imagerotated; //This function threshold the HSV image and create a binary image IplImage* GetThresholdedImage1(IplImage* imghsv) IplImage* imgthresh1=cvcreateimage(cvgetsize(imghsv),ipl_depth_8u, 1); cvinranges(imghsv, cvscalar(107,210,59), cvscalar(119,256,256), imgthresh1); return imgthresh1; //This function threshold the HSV image and create a binary image for another tracking color IplImage* GetThresholdedImage2(IplImage* imghsv) IplImage* imgthresh2=cvcreateimage(cvgetsize(imghsv),ipl_depth_8u, 1); cvinranges(imghsv, cvscalar(30,138,103), cvscalar(39,256,256), imgthresh2); return imgthresh2; //This function threshold the HSV image and create a binary image for the third tracking color IplImage* GetThresholdedImage3(IplImage* imghsv) IplImage* imgthresh3=cvcreateimage(cvgetsize(imghsv),ipl_depth_8u, 1); cvinranges(imghsv, cvscalar(55,194,226), cvscalar(73,256,256), imgthresh3); return imgthresh3; void trackobject1(iplimage* imgthresh1) // Calculate the moments of 'imgthresh' CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments)); cvmoments(imgthresh1, moments, 1); double moment10 = cvgetspatialmoment(moments, 1, 0);

13 double moment01 = cvgetspatialmoment(moments, 0, 1); double area = cvgetcentralmoment(moments, 0, 0); // if the area<1000, I consider that the there are no object in the image and it's because of the noise, the area is not zero if(area>1000) // calculate the position of the ball int posx = moment10/area; int posy = moment01/area; if(lastx1>=0 && lasty1>=0 && posx>=0 && posy>=0) // Draw a yellow line from the previous point to the current point cvline(imgtracking1, cvpoint(posx, posy), cvpoint(lastx1, lasty1), cvscalar(0,0,255), 4); lastx1 = posx; lasty1 = posy; if(area<=1000) lastx1=-1; lasty1=-1; free(moments); void trackobject2(iplimage* imgthresh2) // Calculate the moments of 'imgthresh' CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments)); cvmoments(imgthresh2, moments, 1); double moment10 = cvgetspatialmoment(moments, 1, 0); double moment01 = cvgetspatialmoment(moments, 0, 1); double area = cvgetcentralmoment(moments, 0, 0); // if the area<1000, I consider that the there are no object in the image and it's because of the noise, the area is not zero if(area>1000) // calculate the position of the ball int posx = moment10/area; int posy = moment01/area; if(lastx2>=0 && lasty2>=0 && posx>=0 && posy>=0) // Draw a yellow line from the previous point to the current point cvline(imgtracking2, cvpoint(posx, posy), cvpoint(lastx2, lasty2), cvscalar(0,0,255), 4);

14 lastx2 = posx; lasty2 = posy; if(area<=1000) lastx2=-1; lasty2=-1; free(moments); void trackobject3(iplimage* imgthresh3) // Calculate the moments of 'imgthresh' CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments)); cvmoments(imgthresh3, moments, 1); double moment10 = cvgetspatialmoment(moments, 1, 0); double moment01 = cvgetspatialmoment(moments, 0, 1); double area = cvgetcentralmoment(moments, 0, 0); // if the area<1000, I consider that the there are no object in the image and it's because of the noise, the area is not zero if(area>1000) // calculate the position of the ball int posx = moment10/area; int posy = moment01/area; if(lastx3>=0 && lasty3>=0 && posx>=0 && posy>=0) // Draw a yellow line from the previous point to the current point cvline(imgtracking3, cvpoint(posx, posy), cvpoint(lastx3, lasty3), cvscalar(0,0,255), 4); lastx3 = posx; lasty3 = posy; if(area<=1000) lastx3=-1; lasty3=-1; free(moments); int main() // Setup serial port connection and needed variables.

15 HANDLE hserial = CreateFile(L"COM1", GENERIC_READ GENERIC_WRITE, 0, 0, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0); if (hserial!=invalid_handle_value) printf("port opened! \n"); DCB dcbserialparams; GetCommState(hSerial,&dcbSerialParams); dcbserialparams.baudrate = CBR_9600; dcbserialparams.bytesize = 8; dcbserialparams.parity = NOPARITY; dcbserialparams.stopbits = ONESTOPBIT; SetCommState(hSerial, &dcbserialparams); else if (GetLastError() == ERROR_FILE_NOT_FOUND) printf("serial port doesn't exist! \n"); printf("error while setting up serial port! \n"); char outputchars[]="a"; DWORD btsio; //outputchars[0]='d'; //outputchars[1]='r'; // CvCapture* capture =0; capture = cvcapturefromcam(1); //cvcreatefilecapture(" if(!capture) printf("capture failure\n"); return -1; IplImage* frame=0; frame = cvqueryframe(capture); if(!frame) return -1; //rotate the image frame=rotateimage(frame,180); CvSize size; size=cvgetsize(frame); cout<<size.width<<" "<<size.height;

16 //create a blank image and assigned to 'imgtracking' which has the same size of original video imgtracking1=cvcreateimage(cvgetsize(frame),ipl_depth_8u, 3); imgtracking2=cvcreateimage(cvgetsize(frame),ipl_depth_8u, 3); imgtracking3=cvcreateimage(cvgetsize(frame),ipl_depth_8u, 3); cvzero(imgtracking1); //covert the image, 'imgtracking1' to black cvzero(imgtracking2); //covert the image, 'imgtracking2' to black cvzero(imgtracking3); //covert the image, 'imgtracking3' to black cvnamedwindow("video"); cvnamedwindow("color Tracking1"); cvnamedwindow("color Tracking2"); cvnamedwindow("color Tracking3"); //iterate through each frames of the video while(true) //capture = cvcreatefilecapture(" frame = cvqueryframe(capture); if(!frame) break; //rotate the image //frame=rotateimage(frame,180); frame=cvcloneimage(frame); kernel cvsmooth(frame, frame, CV_GAUSSIAN,3,3); //smooth the original image using Gaussian IplImage* imghsv = cvcreateimage(cvgetsize(frame), IPL_DEPTH_8U, 3); cvcvtcolor(frame, imghsv, CV_BGR2HSV); //Change the color format from BGR to HSV IplImage* imgthresh1 = GetThresholdedImage1(imgHSV); IplImage* imgthresh2 = GetThresholdedImage2(imgHSV); IplImage* imgthresh3 = GetThresholdedImage3(imgHSV); cvsmooth(imgthresh1, imgthresh1, CV_GAUSSIAN,3,3); //smooth the binary image using Gaussian kernel cvsmooth(imgthresh2, imgthresh2, CV_GAUSSIAN,3,3); //smooth the binary image using Gaussian kernel cvsmooth(imgthresh3, imgthresh3, CV_GAUSSIAN,3,3); //smooth the binary image using Gaussian kernel //track the first color trackobject1(imgthresh1); //track the second color trackobject2(imgthresh2); //track the third color trackobject3(imgthresh3); //print the value of lastx and lasty

17 //cout<<lastx1<<" "<<lasty1<<endl; //cout<<lastx2<<" "<<lasty2<<endl; //cout<<lastx3<<" "<<lasty3<<endl; //if SM =0,send different string to Arduino through Xbee according to the detected object1 position //zone1 is cooresponding to x<210 if(sm == 0) if(st == 0) if(lastx1-x1<(-10) && lastx1!=(-1)) outputchars[0]='l'; lasty1!=(-1)) Y1<=10) //zone2 is cooresponding to x>270 if(lastx1-x1>10 && lastx1!=(-1)) outputchars[0]='r'; //zone3 is cooresponding to 210<=x<=270 while y<300 if(lastx1-x1>=(-10) && lastx1-x1<=10 && lasty1-y1<(-10) && outputchars[0]='f'; //zone4 is cooresponding to 210<=x<=270 while y>360 if(lastx1-x1>=(-10) && lastx1-x1<=10 && lasty1-y1>10 && lasty1!=(-1)) outputchars[0]='b'; //zone5 is cooresponding to 210<=x<=270 while 300<=y<=360 if(lastx1-x1>=(-10) && lastx1-x1<=10 && lasty1-y1>=(-10) && lasty1- outputchars[0]='s'; SM=1; int i = 0;

18 //this is for no color detected situation. 'search' mode if(lastx1==(-1) && lasty1==(-1)) outputchars[0]='o'; if(st == 1) outputchars[0]='a'; //if SM=1, send different string to Arduino through Xbee according to the detected object2 position //zone1 is cooresponding to x<210 if(sm == 1) if(st == 0) if(lastx2-x2<(-10) && lastx2!=(-1)) outputchars[0]='l'; lasty2!=(-1)) //zone2 is cooresponding to x>270 if(lastx2-x2>10 && lastx2!=(-1)) outputchars[0]='r'; //zone3 is cooresponding to 210<=x<=270 while y<300 if(lastx2-x2>=(-10) && lastx2-x2<=10 && lasty2-y2<(-10) && outputchars[0]='f'; //zone4 is cooresponding to 210<=x<=270 while y>360 if(lastx2-x2>=(-10) && lastx2-x2<=10 && lasty2-y2>10 && lasty2!=(-1))

19 outputchars[0]='b'; Y2<=10) //zone5 is cooresponding to 210<=x<=270 while 300<=y<=360 if(lastx2-x2>=(-10) && lastx2-x2<=10 && lasty2-y2>=(-10) && lasty2- outputchars[0]='d'; SM=2; //this is for no color detected situation. 'search' mode if(lastx2==(-1) && lasty2==(-1)) outputchars[0]='o'; if(st == 1) outputchars[0]='a'; //if SM =2,send different string to Arduino through Xbee according to the detected object3 position //zone1 is cooresponding to x<210 if(sm == 2) if(st == 0) if(lastx3-x3<(-10) && lastx3!=(-1)) outputchars[0]='l'; //zone2 is cooresponding to x>270 if(lastx3-x3>10 && lastx3!=(-1)) outputchars[0]='r';

20 lasty3!=(-1)) 1)) Y3<=10) //zone3 is cooresponding to 210<=x<=270 while y<300 if(lastx3-x3>=(-10) && lastx3-x3<=10 && lasty3-y3<(-10) && outputchars[0]='f'; //zone4 is cooresponding to 210<=x<=270 while y>360 if(lastx3-230>=(-10) && lastx3-230<=10 && lasty3-y3>10 && lasty3!=(- outputchars[0]='b'; //zone5 is cooresponding to 210<=x<=270 while 300<=y<=360 if(lastx3-x3>=(-10) && lastx3-x3<=10 && lasty3-y3>=(-10) && lasty3- outputchars[0]='s'; SM=3; int i = 0; //this is for no color detected situation. 'search' mode if(lastx3==(-1) && lasty3==(-1)) outputchars[0]='o'; if(st == 1) outputchars[0]='a'; //if SM=3, send different string to Arduino through Xbee according to the detected object2 position //zone1 is cooresponding to x<210 if(sm == 3) if(st == 0)

21 lasty2!=(-1)) Y2<=10) if(lastx2-x2<(-10) && lastx2!=(-1)) outputchars[0]='l'; //zone2 is cooresponding to x>270 if(lastx2-x2>10 && lastx2!=(-1)) outputchars[0]='r'; //zone3 is cooresponding to 210<=x<=270 while y<300 if(lastx2-x2>=(-10) && lastx2-x2<=10 && lasty2-y2<(-10) && outputchars[0]='f'; //zone4 is cooresponding to 210<=x<=270 while y>360 if(lastx2-x2>=(-10) && lastx2-x2<=10 && lasty2-y2>10 && lasty2!=(-1)) outputchars[0]='b'; //zone5 is cooresponding to 210<=x<=270 while 300<=y<=360 if(lastx2-x2>=(-10) && lastx2-x2<=10 && lasty2-y2>=(-10) && lasty2- outputchars[0]='d'; SM=0; //this is for no color detected situation. 'search' mode if(lastx2==(-1) && lasty2==(-1)) outputchars[0]='o';

22 if(st == 1) outputchars[0]='a'; cout<<outputchars<<endl; cout<<sm<<endl; // Add the tracking image and the frame cvadd(frame, imgtracking1, frame); cvadd(frame, imgtracking2, frame); cvadd(frame, imgtracking3, frame); cvshowimage("color Tracking1", imgthresh1); cvshowimage("color Tracking2", imgthresh2); cvshowimage("color Tracking3", imgthresh3); cvshowimage("video", frame); //Clean up used images cvreleaseimage(&imghsv); cvreleaseimage(&imgthresh1); cvreleaseimage(&imgthresh2); cvreleaseimage(&imgthresh3); cvreleaseimage(&frame); //Wait 10mS int c = cvwaitkey(30); //If 'A' is pressed, keep sending 'A' to Arduino Board until 'Z' is pressed if((char)c == 65) ST = 1; if((char)c == 90) ST = 0; //If 'ESC' is pressed, break the loop if((char)c==27 ) break; cvdestroyallwindows() ; cvreleaseimage(&imgtracking1); cvreleaseimage(&imgtracking2); cvreleaseimage(&imgtracking3); cvreleasecapture(&capture); return 0;

23 Arduino code for object tracking, pick up, lay down and obstacle avoidance. #include "DualMC33926MotorShield.h" #include <Servo.h> #include <LiquidCrystal.h> // initialize the library with the numbers of the interface pins LiquidCrystal lcd(40, 41, 42, 43, 44, 45); DualMC33926MotorShield md; Servo myservo1; // create servo object to control a servo Servo myservo2; Servo myservo3; void stopiffault() if (md.getfault()) while(1); void setup() Serial1.begin(9600); //initialize serial md.init(); myservo1.attach(2); // attaches the servo on pin 8 to the servo object myservo2.attach(3); myservo3.attach(5); // set up the LCD's number of columns and rows: lcd.begin(10, 1); //set the variable to store the reading from three IR. int adcvalueleft = 0; int adcvaluemiddle = 0; int adcvalueright = 0; int randomnumb = 0; int randomnumb1 = 0; int randomnumb2 = 0; int randomnumb3 = 0; int randomnumb4 = 0; int ST = 0; int pos1 = 0; // variable to store the servo position int pos2 = 0; int pos3 = 0; //set the variable to store the data sent from the laptop. char getctrl = 'O';

24 void pick_up() // Print a message to the LCD. lcd.setcursor(0, 0); lcd.print("pick up..."); for(pos2 = 93; pos2 >= 41; pos2 -= 1) myservo2.write(pos2); delay(15); for(pos3 = 10; pos3 <= 90; pos3 += 1) myservo3.write(pos3); delay(15); for(pos1 = 100; pos1 > 16; pos1 -= 1) // goes from 0 degrees to 180 degrees // in steps of 1 degree myservo1.write(pos1); // tell servo to go to position in variable 'pos' delay(15); // waits 15ms for the servo to reach the position for(pos2 = 41; pos2 < 94; pos2 += 1) myservo2.write(pos2); delay(15); for(pos3 = 90; pos3>=10; pos3-=1) myservo3.write(pos3); delay(15); for(pos1 = 16; pos1<=100; pos1+=1) myservo1.write(pos1); delay(15); // goes from 180 degrees to 0 degrees // tell servo to go to position in variable 'pos' // waits 15ms for the servo to reach the position void lay_down() // Print a message to the LCD. lcd.setcursor(0, 0); lcd.print("lay down.."); for(pos1 = 100; pos1 > 25; pos1 -= 1) // goes from 0 degrees to 180 degrees // in steps of 1 degree myservo1.write(pos1); // tell servo to go to position in variable 'pos'

25 delay(15); // waits 15ms for the servo to reach the position for(pos2 = 93; pos2 >= 41; pos2 -= 1) myservo2.write(pos2); delay(15); for(pos3 = 10; pos3 <= 90; pos3 += 1) myservo3.write(pos3); delay(15); for(pos1 = 26; pos1<=100; pos1+=1) // goes from 180 degrees to 0 degrees myservo1.write(pos1); // tell servo to go to position in variable 'pos' delay(15); // waits 15ms for the servo to reach the position void obstacle_avoidance() if(adcvalueright > 300) // Print a message to the LCD. lcd.setcursor(0, 0); lcd.print("avoidance."); //digitalwrite(3, HIGH); //turn on the Red LED,cart is turn left md.setspeeds(-100,-100); delay(500); md.setspeeds(-200,0); delay(500); stopiffault(); ST=1; //digitalwrite(3,low); //turn off the LED when finish the turn. //backwards and turn right if the value of the Left side IR get //bigger than 300 if(adcvalueleft > 300) // Print a message to the LCD. lcd.setcursor(0, 0); lcd.print("avoidance."); //digitalwrite(3, HIGH); //turn on the Red LED,cart is turn left md.setspeeds(-100,-100); delay(500); md.setspeeds(0,-200);

26 delay(500); stopiffault(); ST=1; //digitalwrite(3,low); //turn off the LED when finish the turn. //backwards and make a random turn if the value of the middle side IR //get bigger than 300 if(adcvaluemiddle > 300) // Print a message to the LCD. lcd.setcursor(0, 0); lcd.print("avoidance."); //digitalwrite(3, HIGH); //turn on the Red LED,cart is turn left md.setspeeds(-100,-100); delay(500); randomnumb = random(0,2); if(randomnumb == 0) md.setspeeds(0,-200); delay(500); stopiffault(); if(randomnumb == 1) md.setspeeds(-200,0); delay(500); stopiffault(); ST=1; //digitalwrite(3,low); //turn off the LED when finish the turn. void object_tracking() //turn off the robot if receiving 'A' if(getctrl == 'A') // Print a message to the LCD. lcd.setcursor(0, 0); lcd.print("waiting..."); //stop the robot md.setspeeds(0,0); ST=1; //turn right upon receiving 'R' if(getctrl == 'R') // Print a message to the LCD. lcd.setcursor(0, 0);

27 lcd.print("turn right"); digitalwrite(2, HIGH); //Rotate cart RIGHT md.setspeeds(50,-50); //if(!(getctrl='r')) break; ST=1; //turn left upon receiving 'L' if(getctrl == 'L') // Print a message to the LCD. lcd.setcursor(0, 0); lcd.print("turn left."); digitalwrite(3, HIGH); md.setspeeds(-50,50); //if(!(getctrl='l')) break; ST=1; //move forward upon receiving 'F' if(getctrl == 'F') // Print a message to the LCD. lcd.setcursor(0, 0); lcd.print("forward..."); digitalwrite(5, HIGH); //Cart FORWARD md.setspeeds(100,100); //if(!(getctrl='f')) break; ST=1; //move backward upon receiving 'B' if(getctrl == 'B') // Print a message to the LCD. lcd.setcursor(0, 0); lcd.print("backward.."); digitalwrite(3, LOW); digitalwrite(2, LOW); //Cart BACKWARD md.setspeeds(-100,-100); //if(!(getctrl='b')) break; ST=1; //stop the motor upon receiving 'S' if(getctrl == 'S') digitalwrite(5, LOW); digitalwrite(3, LOW);

28 digitalwrite(2, LOW); //Stop cart md.setspeeds(0,0); ST=1; pick_up(); if(getctrl == 'D') md.setspeeds(0,0); ST=1; lay_down(); md.setspeeds(-100,-100); delay(5000); void loop() //read and store the value sent from the laptop while(serial1.available()) //read and store the value of the Left, Middle and Right side IR adcvalueleft=analogread(a0); adcvaluemiddle=analogread(a1); adcvalueright=analogread(a2); //obstacle_avoidance(); //is there anything to read? getctrl = Serial1.read(); //if yes, read it randomnumb1 = random(100,200); randomnumb2 = random(100,200); randomnumb3 = random(0,2); randomnumb4 = random(0,2); object_tracking(); //random route to search for the object up on receiving 'O' if(getctrl == 'O') // Print a message to the LCD. lcd.setcursor(0, 0); lcd.print("search..."); int i = 0; int j = 0; int k = 0; int m = 0; while(i<randomnumb1) k=0; while(k<50) ST = 0;

29 adcvalueleft=analogread(a0); adcvaluemiddle=analogread(a1); adcvalueright=analogread(a2); obstacle_avoidance(); getctrl = Serial1.read(); object_tracking(); if(st == 1) break; md.setspeeds(100,100); k++; if(st == 1) break; i++; if(randomnumb3 == 0) while(j<randomnumb2) m=0; while(m<100) ST = 0; adcvalueleft=analogread(a0); adcvaluemiddle=analogread(a1); adcvalueright=analogread(a2); obstacle_avoidance(); getctrl = Serial1.read(); object_tracking(); if(st == 1) break; md.setspeeds(-100,100); m++; if(st == 1) break; j++;

30 if(randomnumb3 == 1) while(j<randomnumb2) m=0; while(m<100) ST = 0; adcvalueleft=analogread(a0); adcvaluemiddle=analogread(a1); adcvalueright=analogread(a2); obstacle_avoidance(); getctrl = Serial1.read(); object_tracking(); if(st == 1) break; md.setspeeds(100,-100); m++; if(st == 1) break; j++;