Alexander Bryan Shooty The Robot Final Formal report EEL 4665/5666 Intelligent Machines Design Laboratory University of Florida Department of

Transcription

1 Alexander Bryan Shooty The Robot Final Formal report EEL 4665/5666 Intelligent Machines Design Laboratory University of Florida Department of Electrical and Computer Engineering TAs: Andy Gray, Josh Weaver, Nick Cox Instructors: Dr. A. Antonio Arroyo Dr. Eric M. Schwartz

2 Table of Contents Abstract...3 Executive Summary...4 Introduction...4 Integrated System...4 Sensors...6 Conclusion...7 Appendices...7 launcher.h...7 launcher.c...8 db_find.hpp...13 db_find.cpp...14

3 Abstract Shooty the Robot will attempt to play the simple game of darts, throw darts at a target as accurately as possible. Except darts are dangerous and guns are easier to fire. Therefore, the robot will instead fire nerf darts with a USB missile launcher. Executive Summary At boot up, Shooty will begin searching for a target to fire at. The target is known to be an orange circle. For each frame, some thresholding is done and a black and white image is produced with everything that was orange now being white, and everything else black. Then the image is dilated and eroded to remove some noise. Then, contours are found. The minimum bounding circle for the largest contour is then drawn around the contour. The area of the contour and the area of the circle are compared. If they are close, then we assume this is our target as it fits the characteristics we are searching for: a large orange circle. After the target is found, the camera then attempts to center itself on it. Shooty knows where the center of the circle is in each frame, so it's trivial to detect in which direction to move. Actually moving the turret (on which the camera is mounted) is quite easy. A command for move [left right up down ] is given. The turret will move in that direction until it's hardware limit is reached (i.e the turret has moved as far in that direction as the hardware will allow), or when the command for stop is given. When the target has been centered in the image, Shooty must now aim up a certain amount based on how far away the target is, because gravity naturally pulls the missile down. Knowing the radius of the circle, some simple calibration tests performed earlier give Shooty the proper angle of incline needed to perform an accurate shot. After the shot is fired, Shooty will then move the turret all the way down to begin finding the target again. This is done because at times, the angle of inclination needed will be so high that the target is no longer in the frame or is cut off at the bottom and is not recognized as a circle. Introduction It's fun to play darts, but it's even more fun with people. Unfortunately, there are not always people around to play darts with. This is where shooty comes in. Shooty makes darts more fun to play as Shooty plays darts with you. Integrated System Shooty's processing power will be done using a Raspberry Pi. The Pi will do all image processing and will control the missile launcher through USB.

4 The USB missile launcher shown below can be moved, aimed, and fired. It can easily be given commands over USB, which simplifies the design. Images will be captured using a Logitech C270 webcam

5 A 3-cell LiPo battery will be used to power the system. A Raspberry Pi is traditionally powered via USB, and the webcam and missile launcher draw power from the USB port they are connected to. A powered USB hub must be used to power all of these devices as the Raspberry Pi cannot supply enough power to the devices itself. A 5V voltage regulator was connected to the 3 cell LiPo with the output going to the power connector of the powered USB hub. Sensors The Logitech C270 webcam will be used for image detection. The webcam will be used to locate the target, center on it, calculate it's approximate distance away, and aim upwards a certain amount based on how far away the target is. Conclusion The robot was successfully able to identify a target, aim, and fire at the target with reasonable accuracy. The aiming was slow, due the specific method used to move the turret and the low frame rate, but it was accurate and got the job done. The end result was more accurate than I thought it could be be, but also slower. Work could have been done to move the turret a certain distance based on the location of the target in the image, instead of moving a constant step size each frame. This would have sped up the aiming process. Appendices launcher.h #ifndef LAUNCHER_H #define LANUCHER_H #include <unistd.h> #include <time.h> #include <usb.h>

6 #define UP 0x02 #define DOWN 0x01 #define LEFT 0x04 #define RIGHT 0x08 #define FIRE 0x10 #define STOP 0x20 #define DEGS_PER_SEC_LR 50 #define NSEC_PER_DEG_LR /50 #define DEGS_PER_SEC_UD 76 #define NSEC_PER_DEG_UD /76 #define INT_MAX_ void send_command_cheeky(struct usb_device* dev, char cmd); void turn(struct usb_device* dev, char dir, time_t sec, long long nsec); void turn_lr_deg(struct usb_device* dev, char dir, int deg); void turn_up_deg(struct usb_device* dev, char dir, int deg); struct usb_device* find_dev(); char get_code(char *code_str); void a_wait(time_t sec, long nsec); int ts_cmp(struct timespec* a, struct timespec* b); void center_up(struct usb_device* dev); #endif launcher.c #include <stdio.h> #include <stdlib.h> #include <string.h> #include <usb.h> #include "launcher.h" #include <unistd.h> #include <time.h> int debug = 0; void send_command_cheeky(struct usb_device* dev, char cmd) usb_dev_handle *launcher; char data[8]; //some data to be sent over usb int ret;

7 launcher = usb_open(dev); if( launcher == NULL ) perror("unable to open device"); exit(exit_failure); /* Detach kernel driver (usbhid) from device interface and claim */ /* Is this what requires root? */ usb_detach_kernel_driver_np(launcher, 0); usb_detach_kernel_driver_np(launcher, 1); ret = usb_set_configuration(launcher, 1); if (ret < 0) perror("unable to set device configuration"); exit(exit_failure); ret = usb_claim_interface(launcher, 0); if (ret < 0) perror("unable to claim interface"); exit(exit_failure); memset(data, 0, 8); //data will mostly be zeros, so zero it data[0] = 0x02; //has to be done data[1] = cmd; ret = usb_control_msg(launcher, USB_DT_HID, USB_REQ_SET_CONFIGURATION, USB_RECIP_DEVICE, 0,//index data, 8, // Length of data. 5000); // Timeout if(ret!= 8) fprintf(stderr, "Error: %s\n", usb_strerror()); exit(exit_failure); usb_release_interface(launcher, 0); usb_close(launcher);

8 struct usb_device* find_dev() // needs to happen usb_init(); usb_find_busses(); usb_find_devices(); struct usb_bus *busses, *bus; struct usb_device *dev = NULL; busses = usb_get_busses(); printf("did init\n"); //iterate through busses for(bus = busses; bus &&!dev; bus = bus->next) //iterate through devices on current bus for(dev = bus->devices; dev; dev = dev->next) if(dev->descriptor.idvendor == 0x2123 && dev->descriptor.idproduct == 0x1010) printf("found it\n"); return dev; printf("found nothing\n"); return NULL; char get_code(char *code_str) if(strcmp(code_str, "up") == 0) return UP; else if (strcmp(code_str, "down") == 0) return DOWN; else if (strcmp(code_str, "left") == 0) return LEFT; else if (strcmp(code_str, "right") == 0)

9 return RIGHT; else if (strcmp(code_str, "fire") == 0) return FIRE; else if (strcmp(code_str, "stop") == 0) return STOP; else return 0x00; //error code void turn(struct usb_device* dev, char dir, time_t sec, long long nsec) //dir is the hex code relating to left, right, up, or down if(nsec <= INT_MAX_) send_command_cheeky(dev, dir); a_wait(sec, nsec); send_command_cheeky(dev, STOP); else send_command_cheeky(dev, dir); a_wait(sec, INT_MAX_); nsec -= INT_MAX_; while(nsec > INT_MAX_) a_wait(0,int_max_); nsec -= INT_MAX_ ; a_wait(0, nsec); send_command_cheeky(dev, STOP); int ts_cmp(struct timespec* a, struct timespec* b) /** Returns -1, 0, or 1 if a is less than, equal to, or greater than b */ /** Can 0 ever really happen? */

10 if(a->tv_sec < b->tv_sec) return -1; else if (a->tv_sec > b->tv_sec) return 1; else if(a->tv_nsec < b->tv_nsec) return -1; else if(a->tv_nsec > b->tv_nsec) return 1; else return 0; void a_wait(time_t sec, long nsec) /** * Waits the given seconds and nanoseconds, but really only has * resolution to ~1ms. It could have resolution to the nanosecond, * but that would max out a core with the constant polling. Instead, * while the polling occurs, we sleep for a milisecond */ /* * timespec is specified as * struct timespec * time_t tv_sec; seconds * long tv_nsec; nanoseconds * ; */ struct timespec curr, end; clock_gettime(clock_monotonic_raw, &curr); /* * CLOCK_MONOTONIC_RAW isn't affected by timezone changes or admin * changing the system time */

11 end.tv_sec = curr.tv_sec + sec; end.tv_nsec = curr.tv_nsec + nsec; while(ts_cmp(&curr, &end) < 1) /* * sleep for a milisecond, otherwise we max out a core polling for * unncessary resolution */ usleep(1000); clock_gettime(clock_monotonic_raw, &curr); void turn_lr_deg(struct usb_device* dev, char dir, int deg) time_t secs_to_turn = deg / DEGS_PER_SEC_LR; long long degs_rem = deg % DEGS_PER_SEC_LR; long long nsec_to_turn = degs_rem * NSEC_PER_DEG_LR; turn(dev, dir, secs_to_turn, nsec_to_turn); void turn_up_deg(struct usb_device* dev, char dir, int deg) time_t secs_to_turn = deg / DEGS_PER_SEC_UD; long long degs_rem = deg % DEGS_PER_SEC_UD; long long nsec_to_turn = degs_rem * NSEC_PER_DEG_UD; turn(dev, dir, secs_to_turn, nsec_to_turn); void center_up(struct usb_device* dev) printf("centering\n"); turn(dev, DOWN, 1, 0); printf("done centering\n");

12 db_find.hpp #ifndef DB_FIND_H #define DB_FIND_H #define SCALER_LOW.7 #define SCALER_HIGH 1.3 #define RAD_MIN 20 #define COLS 640 #define ROWS 480 #include "opencv2/core/core.hpp" #include "opencv2/features2d/features2d.hpp" #include "opencv2/highgui/highgui.hpp" #include "opencv2/imgproc/imgproc.hpp" #include "opencv2/calib3d/calib3d.hpp" #include "launcher.h" typedef struct db_status unsigned int up : 1; unsigned int down : 1; unsigned int left : 1; unsigned int right : 1; unsigned int just_fired : 1; unsigned int center: 1; struct timespec release_at; db_status; cv::mat thresh_img(cv::mat img, std::vector<int> lowerb, std::vector<int> upperb); int max_contour_area_index( std::vector<std::vector<cv::point> > contours ); bool is_circle(float radius, float calc_area); void set_db_status(db_status*, cv::point2f center, int radius); float rad_to_in(float radius); float in_to_cm(float in); int in_to_deg(float in); void move_launcher(struct usb_device*, db_status*); int rad_to_deg(float radius);

13 void check_fire(struct usb_device*, db_status*); #endif db_find.cpp #include <stdio.h> #include <iostream> #include <unistd.h> #include <usb.h> #include "db_find.hpp" #include "launcher.h" #include <math.h> #include <cstdlib> using namespace std; using namespace cv; float y_tilt_sec; float radius; int main() //launcher init struct usb_device* launch_dev = find_dev(); if (launch_dev == NULL) fprintf(stderr, "ERROR: couldn't find device\n"); exit(exit_failure); //opencv init VideoCapture cap(0); namedwindow("cam", CV_WINDOW_AUTOSIZE); namedwindow("threshed", CV_WINDOW_AUTOSIZE); Mat frame; Mat framehsv; Mat threshed_img; Mat approx_curve; Mat final = Mat::zeros(ROWS, COLS, CV_8UC3); Point2f center; db_status db_stat = 0, 0, 0, 0, 0, 0; int dbrad;

14 int dbcenterx; int dbcentery; vector<vector<point> > contours; vector<vec4i> hierarchy; int max_index; int lowerb[] = 0, 115, 115; int upperb[] = 17, 255, 255; vector<int> upperb_vect(upperb, upperb + sizeof(upperb)/sizeof(int)); vector<int> lowerb_vect(lowerb, lowerb + sizeof(lowerb)/sizeof(int)); center_up(launch_dev); char key = 'a'; while(1) try cap >> frame; //grab current frame and put it into frame //convert to HSV space cvtcolor(frame, framehsv, CV_BGR2HSV); //thresh image threshed_img = thresh_img(framehsv, lowerb_vect, upperb_vect); imshow("threshed", threshed_img); //grab contours findcontours( threshed_img, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) ); max_index = max_contour_area_index( contours ); Scalar color( 255, 255, 255 ); //drawcontours( threshed_img, contours, max_index, color, CV_FILLED, 8, // hierarchy ); check_fire(launch_dev, &db_stat); if(contours.size() > 0) approxpolydp(contours[max_index], approx_curve, 3, true); minenclosingcircle(approx_curve, center, radius); dbrad = (int)radius;

15 if(dbrad > RAD_MIN && is_circle(radius, contourarea(contours[max_index]))) printf("radius:\t%f\n", radius); Scalar color2(0, 255, 255); circle( final, center, (int)dbrad, color2, 2, 8, 0); imshow("cam", final); set_db_status(&db_stat, center, dbrad); move_launcher(launch_dev, &db_stat); final = Mat::zeros(ROWS, COLS, CV_8UC3); //imshow("cam", frame); key = waitkey(10); if(key == 27) break; else if(key == 's') imwrite("pic.png", frame); printf("took pic\n"); catch (exception& e) printf("gay exception\n\n"); continue; return 0; Mat thresh_img(mat img, vector<int> lowerb, vector<int> upperb) /* * Takes in HSV image and returns thresholded version */ Mat threshed_image; inrange(img, lowerb, upperb, threshed_image); erode(threshed_image, threshed_image, Mat());

16 dilate(threshed_image, threshed_image, Mat(), Point(-1, -1), 3); return threshed_image; int max_contour_area_index( vector<vector<point> > contours ) float curr_area, max_area = 0; int max_area_i = 0; for(int i = 0; i < contours.size(); i++) curr_area = contourarea(contours[i], false); if(curr_area > max_area) max_area = curr_area; max_area_i = i; return max_area_i; bool is_circle(float radius, float area_guess) float area_calc = M_PI*powf(radius, 2); if( area_calc < SCALER_LOW * area_guess) return false; else if( area_calc > SCALER_HIGH * area_guess ) return false; else return true; void set_db_status(db_status* db_status_p, Point2f center, int radius) /* * Sets position of db relative to screen. * (0,0) is at top left, not botton left like in a normal cartesian plane */

17 //horizontal if( center.x < COLS/2-10 ) db_status_p->left = 1; db_status_p->right = 0; else if( center.x > COLS/ ) db_status_p->right = 1; db_status_p->left = 0; else db_status_p->right = 0; db_status_p->left = 0; //vertical if( center.y < ROWS/2-10 ) db_status_p->up = 1; db_status_p->down = 0; else if( center.y > ROWS/ ) db_status_p->down = 1; db_status_p->up = 0; else db_status_p->down = 0; db_status_p->up = 0; if(db_status_p->left db_status_p->right db_status_p->up db_status_p->down) db_status_p->center = 0; else db_status_p->center = 1; void move_launcher(struct usb_device* dev, db_status* db_stat) if(db_stat->just_fired)

18 return; else if(db_stat->center) float in = rad_to_in(radius); int deg = rad_to_deg(radius); printf("radius: %f\tdeg: %d\n", radius, deg); struct timespec tmp; clock_gettime(clock_monotonic_raw, &tmp); turn_up_deg(dev, UP, deg); clock_gettime(clock_monotonic_raw, &tmp); send_command_cheeky(dev, FIRE); //grab curr time and add 3 secs approx time to fire clock_gettime(clock_monotonic_raw, &tmp); tmp.tv_sec += 3; db_stat->release_at = tmp; db_stat->just_fired = 1; printf("locking\n"); printf("%lu\n", tmp.tv_sec); else if(db_stat->left) printf("need to turn left\n"); //turn_lr_deg(dev, LEFT, 1); turn(dev, LEFT, 0, /2); //.005 sec db_stat->left = 0; else if(db_stat->right) printf("need to turn right\n"); //turn_lr_deg(dev, RIGHT, 1); turn(dev, RIGHT, 0, /2); //.005 sec db_stat->right = 0; else if(db_stat->up) printf("need to turn up\n"); //turn_up_deg(dev, UP, 1); turn(dev, UP, 0, ); //.01 sec

19 db_stat->up = 0; y_tilt_sec +=.01; else if(db_stat->down) printf("need to turn down\n"); //turn_up_deg(dev, DOWN, 1); turn(dev, DOWN, 0, ); //.01 sec db_stat->down = 0; y_tilt_sec -=.01; void check_fire(struct usb_device* dev, db_status* db_stat) if(db_stat->just_fired) struct timespec now; clock_gettime(clock_monotonic_raw, &now); //if release_at time has passed if( ts_cmp( &(db_stat->release_at), &now ) <= 0 ) printf("releasing\n"); printf("%lu\n",now.tv_sec); center_up( dev ); db_stat->just_fired = 0; float rad_to_in(float radius) /* Returns how far away the db is in inches */ return (radius )/(-6.2); float in_to_cm(int in) return in*2.54; int in_to_deg(float in) return round((in - 3.5)/(9.1)); int rad_to_deg(float radius)

20 if(radius > 84 && radius < 96) return 6; else if(radius > 40 && radius < 55) return 20; else if(radius > 80 && radius <= 110) return 20; else if(radius > 110 && radius < 140) return 50; else return 8;