/** * Runs the motors with arcade steering. */ void OperatorControl(void) { HSLImage *Himage; Threshold targetThreshold(247, 255, 60, 140, 10, 50); BinaryImage *matchingPixels; vector<ParticleAnalysisReport> *pReport; //myRobot->SetSafetyEnabled(true); Saftey->SetEnabled(false); AxisCamera &mycam = AxisCamera::GetInstance("10.15.10.11"); mycam.WriteResolution(AxisCamera::kResolution_640x480); mycam.WriteCompression(20); mycam.WriteBrightness(25); Wait(3.0); dsLCD = DriverStationLCD::GetInstance(); dsLCD->Clear(); float X[2]; float Y[2]; float Z[2]; while(IsOperatorControl()) { X[1] = Stick1->GetX(); X[2] = Stick2->GetX(); Y[1] = Stick1->GetY(); Y[2] = Stick2->GetY(); Z[1] = Stick1->GetZ(); Z[2] = Stick2->GetZ(); Jaguar1->Set(Y[1]); Jaguar2->Set(Y[2]); Wait(0.005); if (mycam.IsFreshImage()) { Himage = mycam.GetImage(); matchingPixels = Himage->ThresholdHSL(targetThreshold); pReport = matchingPixels->GetOrderedParticleAnalysisReports(); for (unsigned int i = 0; i < pReport->size(); i++) { printf("Index: %d X Center: %d Y Center: %d \n", i, (*pReport)[i].center_mass_x, (*pReport)[i].center_mass_y); } delete Himage; delete matchingPixels; delete pReport; } } //myRobot->ArcadeDrive(stick); // drive with arcade style (use right stick) //Wait(0.005); // wait for a motor update time }
bool tracking (bool use_alternate_score) //camera tracking function { //takes one camera frame and turns towards tallest target //returns true if target is within deadzone, returns false otherwise Threshold tapeThreshold(0, 255, 0, 90, 220, 255); //red hsl as of 20110303, this is the hue, saturation and luminosicity ranges that we want BinaryImage *tapePixels;// Image *convexHull; BinaryImage *convexHullBinaryImage; ParticleAnalysisReport par;//analyzed blob (pre convex hull) ParticleAnalysisReport convexpar;// ONE filled-in blob vector<ParticleAnalysisReport>* pars;//where many analyzed blob goes (pre) vector<ParticleAnalysisReport>* convexpars; //where MANY filled-in blobs go bool foundAnything = false; double best_score = 120; double best_speed; double particle_score; ImageType t; int bs; img = cam->GetImage(); printf("cam->GetImage() returned frame %d x %d\n",img->GetWidth(),img->GetHeight()); tapePixels = img->ThresholdHSL(tapeThreshold); imaqGetBorderSize(tapePixels->GetImaqImage(),&bs); imaqGetImageType(tapePixels->GetImaqImage(),&t); convexHull = imaqCreateImage(t,bs); convexHullBinaryImage = new BinaryImageWrapper(convexHull); convexHullBinaryImage->GetOrderedParticleAnalysisReports(); //tapePixels = img->ThresholdHSL(int 0,int 50,int -100,int -50,int luminenceLow,int luminanceHigh); pars = tapePixels->GetOrderedParticleAnalysisReports(); imaqConvexHull(convexHull,tapePixels->GetImaqImage(),true); convexHullBinaryImage = new BinaryImageWrapper(convexHull); convexpars = convexHullBinaryImage->GetOrderedParticleAnalysisReports(); //imaqGetParticleInfo() //convexpars = convexHull->GetOrderedParticleAnalysisReports(); for (int i=0;i < convexHullBinaryImage->GetNumberParticles();i++) { //par = (*pars)[0]; //convexpar = (*convexpars)[i]; convexpar = convexHullBinaryImage->GetParticleAnalysisReport(i); par = tapePixels->GetParticleAnalysisReport(i); if((convexpar.boundingRect.width < 10) || (convexpar.boundingRect.height < 7)) { continue; } // printf("%d par:%f convex:%f particle area\n",i,par.particleArea,convexpar.particleArea); if ((par.particleArea/convexpar.particleArea > 0.4)) { printf("%d skip max fillness ratio\n",i); continue; } if ((par.particleArea/convexpar.particleArea < 0.10)) { printf("%d skip min fillness ratio\n",i); continue; } if((double)(convexpar.boundingRect.width)/(double)(convexpar.boundingRect.height)>1.8) { printf("%d skip max aspect ratio\n",i); continue; } if((double)(convexpar.boundingRect.width)/(double)(convexpar.boundingRect.height)<.8) { printf("%d skip min aspect ratio\n",i); continue; } //printf("%f center of mass x\n",par.center_mass_x_normalized); //printf("%f center of mass y\n",par.center_mass_y_normalized); distanceInInches = (18.0*179.3)/(convexpar.boundingRect.height); double pwidth = convexpar.boundingRect.width; double mwidth = ((double)convexpar.boundingRect.left+(double)convexpar.boundingRect.width*0.5); double angle = ((180.0/3.14159)*acos (pwidth * distanceInInches/179.3/24.0) ); if(angle != angle) angle = 0.0; // if angle is NaN, set to zero printf("%f distance in inches\n",distanceInInches); //printf("%f angle\n",(180.0/3.14159)*acos (pwidth * distanceInInches/415.0/24.0) ); printf("%d BBctrX:%f CMX:%f\n", i, (double)convexpar.boundingRect.left + (double)convexpar.boundingRect.width*0.5, (double)par.center_mass_x); //printf("%f angle2\n",(((pwidth * distanceInInches)/415.0)/24.0)); //printf("%f center of mass x\n",par.center_mass_x_normalized); printf("%d %f %f center of mass x\n",i,convexpar.center_mass_x_normalized,par.center_mass_x_normalized); printf("%d %f %f center of mass y\n",i,convexpar.center_mass_y_normalized,par.center_mass_y_normalized); printf("%d %f %f rectangle score\n",i,(convexpar.particleArea)/((convexpar.boundingRect.width)*(convexpar.boundingRect.height))*(100),(par.particleArea)/((par.boundingRect.width)*(par.boundingRect.height))*(100)); printf("%d %f fillness ratio\n",i,par.particleArea/convexpar.particleArea); printf("%d %d %d width and height\n",i,(convexpar.boundingRect.width),(convexpar.boundingRect.height)); printf("%d %f aspect ratio\n",i,((convexpar.boundingRect.width)/(double)(convexpar.boundingRect.height))); if ((double)(par.center_mass_x)>mwidth) { angle=angle*(-1.0); } printf("%f true angle\n",angle); //Wait(1.0); double aiming_target_offset = 0.0; //aiming_target_offset = pwidth * angle * (-0.5 / 45.0); numbers are iffy -> NaN double speed = trackingFeedbackFunction(mwidth + aiming_target_offset - 80.0); printf("%f aiming_target_offset due to %f degree angle\n", aiming_target_offset, angle); printf("%f x offset \n",mwidth + aiming_target_offset - 80.0); printf("%f speed \n", speed); foundAnything = true; if (use_alternate_score == false){ particle_score = convexpar.center_mass_y; } else{ particle_score = 2.0*fabs((double)convexpar.center_mass_y - 60.0) + fabs((double)convexpar.center_mass_x - 80.0); } // keep track of the *lowest* score if (best_score > particle_score) { best_score = particle_score; best_speed = speed; } } if(foundAnything == false) { myRobot->TankDrive(0.0, 0.0); } else { myRobot->TankDrive(-best_speed,best_speed); } delete img; delete tapePixels; delete pars; delete convexHullBinaryImage; delete convexpars; //imaqDispose(convexHull); if (foundAnything && best_speed == 0.0){ return true; } else { return false; } }
void OperatorControl(void) { myRobot.SetSafetyEnabled(false); AxisCamera &camera = AxisCamera::GetInstance("10.28.53.11"); camera.WriteResolution(AxisCamera::kResolution_320x240); camera.WriteRotation(AxisCamera::kRotation_180);//Flip image upside-down. camera.WriteCompression(80);//Compresses the image(?) camera.WriteBrightness(50);//Sets the brightness to 80 on a scale from 0-100 DriverStationLCD *screen = DriverStationLCD::GetInstance(); int count = 0; while(IsOperatorControl()) { screen->UpdateLCD(); count++; HSLImage* imgpointer; //declares a new hue saturation lum image imgpointer = camera.GetImage(); //grabs an image to initialize that image //imaqCreateImage(IMAQ_IMAGE_U8,) //imaqCast(NULL, imgpointer, IMAQ_IMAGE_U8, NULL, -1); BinaryImage* binImg = NULL; //declares a new binary image //ThresholdHSL changes our regular image into a binary image. /*hueLow Low value for hue hueHigh High value for hue saturationLow Low value for saturation saturationHigh High value for saturation luminenceLow Low value for luminence luminenceHigh High value for luminence */ binImg = imgpointer->ThresholdHSL(1, 255, 1, 255, 230, 255); //Saves the image to a temp directory binImg->Write("/tmp/thresh.jpg"); //Writes the binary image to disk imaqWriteFile(binImg->GetImaqImage(), "/tmp/thresh2.jpg", NULL); delete imgpointer; Image* Convex = imaqCreateImage(IMAQ_IMAGE_U8, 0); int returnvalue; returnvalue = imaqConvexHull(Convex, binImg->GetImaqImage(), TRUE); // imaqWriteFile(Convex, "/tmp/convex.jpg", NULL); delete binImg; // screen->PrintfLine(DriverStationLCD::kUser_Line4,"convex is %d",returnvalue); screen->UpdateLCD(); float lookuptable[256]; lookuptable[0] = 0; lookuptable[1] = 65535; //Converts image to 16 bit, then back to 8 bit. Image* cast = imaqCreateImage(IMAQ_IMAGE_U16, 0); imaqCast(cast, Convex, IMAQ_IMAGE_U16, lookuptable, 0); imaqDispose(Convex); Image* bitcast = imaqCreateImage(IMAQ_IMAGE_U8, 0); imaqCast(bitcast, cast, IMAQ_IMAGE_U8, NULL, 0); imaqDispose(cast); // screen->PrintfLine(DriverStationLCD::kUser_Line3,"det %d", imaqGetLastError());//Notifies the user imaqWriteFile(bitcast, "/tmp/bitcast.jpg", NULL); //Image* SuperSize = im //imaqCreateImage(IMAQ_IMAGE_U8, 2240); //int returnvalue2; //returnvalue2 = imaqSizeFilter(SuperSize, Convex, TRUE, 1, IMAQ_KEEP_LARGE, NULL); //screen->UpdateLCD(); //imaqDispose (Convex); // ROI *roi; // Rect rectangle; // rectangle.top = 0; // rectangle.left = 0; // rectangle.width = 320; // rectangle.height = 120; // imaqAddRectContour(roi,rectangle); static RectangleDescriptor rectangleDescriptor = { 30, // minWidth (All values are estimated) 200, // maxWidth 20, // minHeight 200 // maxHeight }; static CurveOptions curveOptions = //extraction mode specifies what the VI identifies curves in the image. curve options are all the { IMAQ_NORMAL_IMAGE, // extractionMode 75, // threshold IMAQ_NORMAL, // filterSize 25, // minLength 15, // rowStepSize 15, // columnStepSize 10, // maxEndPointGap FALSE, // onlyClosed FALSE // subpixelAccuracy }; static ShapeDetectionOptions shapeOptions = { IMAQ_GEOMETRIC_MATCH_ROTATION_INVARIANT, // mode NULL, // angle ranges 0, // num angle ranges {75, 125}, // scale range 300 // minMatchScore }; int matches = 0; //double highscore = 0; //int highestindex = -1; float difference = 0; float time = 0; float average = 0; double y = 0; //The big important line of code that does important stuff RectangleMatch* recmatch = imaqDetectRectangles(bitcast, &rectangleDescriptor, &curveOptions, &shapeOptions, NULL, &matches); // DashboardDataSender *dds; // dds = new DashboardDataSender; //screen->PrintfLine(DriverStationLCD::kUser_Line3,"det %d", imaqGetLastError()); //screen->PrintfLine(DriverStationLCD::kUser_Line4,"Matches: %d",matches);//Notifies the user screen->PrintfLine(DriverStationLCD::kUser_Line1,"Matches: %i",matches);//Notifies the user screen->UpdateLCD(); /*for(int i = 0; i < matches; i++) { //screen->PrintfLine((DriverStationLCD::Line)(i+1),"%i,%i,%i",recmatch[i].height,recmatch[i].width,recmatch[i].score); if(recmatch[i].score > highscore) { highscore = recmatch[i].score; highestindex = i; } screen->PrintfLine(DriverStationLCD::kUser_Line1,"score %i, i %i", recmatch[i].score, i); screen->UpdateLCD(); }*/ average = (recmatch->corner[1].x + recmatch->corner[3].x)/2; y = (472/(recmatch->height-4)); screen->PrintfLine(DriverStationLCD::kUser_Line5,"%f",average); screen->PrintfLine(DriverStationLCD::kUser_Line3," %f",distance(recmatch->height)); //screen->PrintfLine(DriverStationLCD::kUser_Line5,"%f,%f",recmatch->height, y); screen->PrintfLine(DriverStationLCD::kUser_Line6,"%f,%f",ceil(recmatch->corner[1].x),ceil(recmatch->corner[3].x)); screen->UpdateLCD(); //screen->PrintfLine(DriverStationLCD::kUser_Line6,"%d, %d, %d, %d",recmatch->corner[1].x,recmatch->corner[2].x,recmatch->corner[3].x,recmatch->corner[4].x); //screen->PrintfLine(DriverStationLCD::kUser_Line5,"%d, %d",recmatch->height,recmatch->width); if(average == 0) { screen->PrintfLine(DriverStationLCD::kUser_Line1,"Image Not Found"); } else if(average < 145) { difference = (160 - average); time = difference*0.0062; vic.Set(0.1); Wait(time); vic.Set(0.0); screen->PrintfLine(DriverStationLCD::kUser_Line1,"Less!"); screen->UpdateLCD(); } else if (average > 175) { difference = (average - 160); time = difference*0.0062; vic.Set(-0.1); Wait(time); vic.Set(0.0); screen->PrintfLine(DriverStationLCD::kUser_Line1,"More!"); screen->UpdateLCD(); } else { vic.Set(0); screen->PrintfLine(DriverStationLCD::kUser_Line1,"Perfection!"); screen->UpdateLCD(); } screen->PrintfLine(DriverStationLCD::kUser_Line2,"Time: %f",time); screen->PrintfLine(DriverStationLCD::kUser_Line4,"difference %f",difference); // screen->PrintfLine(DriverStationLCD::kUser_Line2,"Score %i, Index %i", highscore, highestindex); /* if(matches > 0) { int counter = 0; while (counter < 20) { average = (recmatch[highestindex].corner[1].x + recmatch[highestindex].corner[2].x + recmatch[highestindex].corner[3].x +recmatch[highestindex].corner[4].x)/4; average2 = (recmatch[highestindex].corner[1].y + recmatch[highestindex].corner[2].y + recmatch[highestindex].corner[3].y +recmatch[highestindex].corner[4].y)/4; // screen->PrintfLine(DriverStationLCD::kUser_Line5,"Center = %f, %f",average, average2); screen->UpdateLCD(); if (average > 170) { vic.Set(-0.1); //screen->PrintfLine(DriverStationLCD::kUser_Line1,"Left %f",vic.Get()); //screen->UpdateLCD(); } else if (average < 150) { vic.Set(0.1); //screen->PrintfLine(DriverStationLCD::kUser_Line1,"Right %f",vic.Get()); //screen->UpdateLCD(); } else { counter = 20; vic.Set(0); // screen->PrintfLine(DriverStationLCD::kUser_Line6,"Not Mallory! %f",vic.Get()); screen->UpdateLCD(); } counter++a; } */ // imaqWriteFile(cast, "/tmp/linedetect.jpg", NULL); // } /* else { vic.Set(0); // screen->PrintfLine(DriverStationLCD::kUser_Line6,"Not Mallory! %f",vic.Get()); screen->UpdateLCD(); }*/ imaqDispose(bitcast); imaqDispose(recmatch); //imaqDispose(roi); vic.Set(0); } }