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;
}
