Skip to content

Instantly share code, notes, and snippets.

/pledge.cpp Secret

Created Jun 16, 2016
Embed
What would you like to do?
double detection::pledge(int sit, double &counter, double &targetFlag, double &pledgeFlag){
// This code is designed to make the robot move into a certain direction until an object is 'hit'. After 'hitting' the object, PICO will keep his right 'hand' to the wall and follow it untill the sum of its corners (left being negative and right being positive) equals zero. When the sum equals zero PICO will keep heading in one direction untill 'hitting' another object. This is known as the pledge algorithm.
// movementDir must be set to 0 after making a movement (e.g. after turning right)
// The code must check for possible doors before evaluating the situation and making a decision (e.g. previously find a dead end, first check if its not a door and secretly a straight, then give the 'sit = 2'-flag to this code)
double movementDir; //direction in which must be moved
std::cout << "Counter pledge : " << counter << std::endl;
if(counter == 0) { // PICO has not jet accounted an object in its trajectory
if(sit == 0 || sit == -1 || sit == 1 || sit == 4 ) { // sit == 0 is straight, sit == -1 is junction on left, sit == 1 is junction on right, sit == 4 crossing
movementDir = 0; // movementDir = 0 means go straight
}
else if(sit == -2 || sit == 3) { // sit == -2 is left corner, sit == 3 is T-junction
movementDir = 1; // movementDir = 1 means go left
counter = counter - 1; // one left turn
std::cout << "PLEDGE LEFT : " << counter << std::endl;
}
else if(sit == 5 || sit == 2) { // sit == 5 is dead end, sit == 2 is right corner
movementDir = 3; // movementDir = 3 means turn around
counter = counter - 2; // two left turns
}
}
else if (counter < 0){ // PICO has accounted an object and is using the right hand rule untill counter is 0
if(sit == 0 || sit == -1) { // sit == 0 is straight, sit == -1 is junction on left
movementDir = 0; // movementDir = 0 means go straight
}
else if(sit == -2 ) { // sit == -2 is left corner
movementDir = 1; // movementDir = 1 means go left
counter = counter - 1; // one left turn
}
else if(sit == 2 || sit == 1 || sit == 4 || sit == 3) { // sit == 2 is right corner, sit == 1 is junction on right, sit == 4 crossing, sit == 3 is T-junction
movementDir = 2; // movementDir = 2 means go right
counter = counter + 1; // one right turn
}
else if(sit == 5 ) { // sit == 5 is dead end
movementDir = 3; // movementDir = 3 means turn around
counter = counter - 2; // two left turns
}
}
else {
counter = 0; // in case of a mistake
}
pledgeFlag = 1; // ensures that the correct code will be executed
std::cout << "Counter after pledge: " << counter << std::endl;
return movementDir;
// Overview of movementDir and sit values
// movementDir, 0 = straight, 1 = left, 2 = right, 3 = turn around
// sit, 0 = straight, 5 = dead end, -2 = left corner, 2 = right corner, -1 = exit on the left, 1 = exit on the right, 4 = crossing, 3 = T-junction
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment