Robot() { initStructuralConstraints(); initTexture(); initLights(); initHip(); initBust(); initNeck(); initHead(); initLeftShoulder(); initRightShoulder(); initLeftUpperArmFront(); initLeftLowerArmFront(); initRightUpperArmFront(); initRightLowerArmFront(); initLeftUpperArmBehind(); initLeftLowerArmBehind(); initRightUpperArmBehind(); initRightLowerArmBehind(); initLeftFrontHand(); initRightFrontHand(); initLeftBehindHand(); initRightBehindHand(); initLeftUpperLeg(); initLeftLowerLeg(); initRightUpperLeg(); initRightLowerLeg(); initLeftFoot(); initRightFoot(); behindArmAngle=0; keys = KeyControls(); }
// -------------------------------------------------------------------------- // main(Number of arguments, Argument values) // Description : This is the entry point of the program. // Return value : SUCCESS:0 ERROR:-1 // -------------------------------------------------------------------------- int main(int argc, char **argv) { // Initialize // If drone is not connected, initialise webcam if (!ardrone.open()) { printf("Drone failed to connect.\n"); isDroneConnected = false; } else isDroneConnected = true; //// DEBUGGING //isDroneConnected = false; quitProgram = false; int patternCount = 0; visiblePattern = 0; //lastVisiblePattern = 0; absoluteControl = false; vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0; points = 0, pSaved = 0, cSaved = 0; peoplePicked = false, cratePicked = false; gameTimeStart = cvGetTickCount(); gameTimeLeft = 0; gameOn = false; // Loading patterns LoadPattern(filename1, patternLibrary, patternCount); LoadPattern(filename2, patternLibrary, patternCount); LoadPattern(filename3, patternLibrary, patternCount); LoadPattern(filename4, patternLibrary, patternCount); LoadPattern(filename5, patternLibrary, patternCount); LoadPattern(filename6, patternLibrary, patternCount); LoadPattern(filename7, patternLibrary, patternCount); LoadPattern(filename8, patternLibrary, patternCount); LoadPattern(filename9, patternLibrary, patternCount); LoadPattern(filename10, patternLibrary, patternCount); LoadPattern(filename11, patternLibrary, patternCount); cout << patternCount << " patterns are loaded." << endl; // Pattern detector arguments int norm_pattern_size = PAT_SIZE; double fixed_thresh = 40; double adapt_thresh = 5;//non-used with FIXED_THRESHOLD mode int adapt_block_size = 45;//non-used with FIXED_THRESHOLD mode double confidenceThreshold = 0.35; int mode = 2;//1:FIXED_THRESHOLD, 2: ADAPTIVE_THRESHOLD // Set to vertical drone camera ardrone.setCamera(1); // Initialise PatternDetector PatternDetector myDetector(fixed_thresh, adapt_thresh, adapt_block_size, confidenceThreshold, norm_pattern_size, mode); vector<Point2f> coordinates; Point2f point; coordinates.push_back(point); coordinates.push_back(point); coordinates.push_back(point); coordinates.push_back(point); coordinates.push_back(point); for (int i = 0; i <= patternCount; i++) { // Initialise each pattern coordinates patternsCoordinates.push_back(coordinates); //// Initialise each pattern timer //int timer; //timers.push_back(timer); } // Start main loop while (1) { // check to terminate program if (quitProgram) break; // Update if (!ardrone.update()) break; // OpenCV image IplImage *img; // Check which image source to feed to OpenCV if (isDroneConnected) { // Get drone image img = ardrone.getImage(); } else { // Capture webcam feed webcamCapture = cvCaptureFromCAM(0); // Get webcam image img = cvQueryFrame(webcamCapture); } // Create image matrix Mat imgMat = Mat(img); // Render the HUD Mat result; if (isDroneConnected) result = HUD(imgMat, WIDTH, HEIGHT); else result = HUD(imgMat, WIDTH, WEBCAM_HEIGHT); // Create a buffer of the image Mat buffer; result.copyTo(buffer); // Run the detector myDetector.detect(imgMat, cameraMatrix, distortions, patternLibrary, detectedPattern); // Augment the input frame (and print out the properties of pattern if you want) if (detectedPattern.size()) { for (unsigned int i = 0; i < detectedPattern.size(); i++) { // Get pattern id int id = detectedPattern[i].id; //// Start current pattern seen timer //int now = cvGetTickCount(); //int passedSinceSeen = ((now - timers[detectedPattern[i].id]) / (cvGetTickFrequency() * 1000)) / 1000; //// Only set visible pattern if detected a pattern for more than 1 second //if (passedSinceSeen > SEEN_TIMER) SetVisiblePattern(detectedPattern[i].id); SetVisiblePattern(id); // Drawing detectedPattern.at(i).draw(buffer, cameraMatrix, distortions); // Get pattern corner and centre coordinates Point2f ul, ur, lr, ll, centre; detectedPattern.at(i).getCoordinates(ul, ur, lr, ll, centre, cameraMatrix, distortions); // Store coordinates patternsCoordinates[id][0] = ul; patternsCoordinates[id][1] = ur; patternsCoordinates[id][2] = lr; patternsCoordinates[id][3] = ll; patternsCoordinates[id][4] = centre; if (isDroneConnected) CheckGamePatterns(WIDTH, HEIGHT, id); else CheckGamePatterns(WIDTH, WEBCAM_HEIGHT, id); } } else { //for (int i = 0; i < patternCount; i++) { // // reset pattern timers // timers[i] = cvGetTickCount(); //} // Reset visible pattern visiblePattern = 0; } //// Get elapsed time since last saw pattern //lastVPElapsed = cvGetTickCount(); //int elapsedTime = ((lastVPElapsed - lastVPStart) / (cvGetTickFrequency() * 1000)) / 1000; //// Check if reset timer limit passed and reset last visible pattern //if (elapsedTime < 300) // if (RESET_TIMER - elapsedTime < 0) lastVisiblePattern = 0; // Get key input int key = cvWaitKey(33); // Providing key controls KeyControls(key); // Autonomous drone control KeepGoodAltitude(); AutoAdjustPosition(key); // Always go back to hovering if no user input, no pattern visible and too old last visible pattern (0) if (absoluteControl && key < 0) Hover(); else if (key < 0 && visiblePattern == 0/* && lastVisiblePattern == 0*/) Hover(); // Drone movement ardrone.move3D(vx, vy, vz, vr); // Check game over if (gameTimeLeft < 0) { gameOn = false; peoplePicked = false; cratePicked = false; } // Combine buffer with original image + opacity double opacity = 0.4; addWeighted(buffer, opacity, result, 1 - opacity, 0, result); // Initialise window with OpenGL support namedWindow("AR.Drone", WINDOW_OPENGL); // Show the OpenCV image in a new window AR.Drone imshow("AR.Drone", result); // Give HighGUI time to process the draw requests cvWaitKey(1); // Clear pattern for next tick detectedPattern.clear(); } // Stop processes before quitting Stop(); return 0; }