int no_wall_right(){
int i = 0;
double wall = 0;
for ( i = 0; i < 25; i++)
{
wall += get_front_ir_dist(RIGHT);
}
return (wall/25 < 35) ? 0 : 1;
}
int parallel(double *curr_coord){
int i = 0;
double leftir;
double rightir;
int parallel = 0;
double error = 0.04 ;
double time = 30.0;
set_ir_angle(LEFT, 45);
set_ir_angle(RIGHT, -45);
while (!parallel){
for ( i = 0; i < time; i++)
{
leftir += get_front_ir_dist(LEFT);
rightir+= get_front_ir_dist(RIGHT);
}
leftir = leftir/time;
rightir = rightir/time;
if(leftir/time > rightir/time + error){
set_motors(1, -1);
position_tracker(curr_coord);
for ( i = 0; i < time; i++){
leftir += get_front_ir_dist(LEFT);
rightir += get_front_ir_dist(RIGHT);
}
leftir = leftir/time;
rightir = rightir/time;
}
else if(rightir/time > leftir/time + error){
set_motors(-1, 1);
position_tracker(curr_coord);
for (i = 0; i < time; i++){
leftir += get_front_ir_dist(LEFT);
rightir += get_front_ir_dist(RIGHT);
}
leftir = leftir/time;
rightir = rightir/time;
}
else{
parallel = 1;
set_motors(0, 0);
}
printf(" ### Paralleling LEFTIR %f, RIGHTIR %f \n", leftir/time, rightir/time);
usleep(300000);
set_motors(0, 0);
position_tracker(curr_coord);
}
set_ir_angle(LEFT, -45);
set_ir_angle(RIGHT, 45);
usleep(20000);
}
void findAdjacentNodes(int currentNode, int orientation, int adjacentNodes[]) {
// Special case if called from the starting cell
if (currentNode == 0) {
adjacentNodes[0] = -1;
adjacentNodes[1] = 1;
adjacentNodes[2] = -1;
return;
}
int sampleSize = 40;
int frontSampleSize = 5;
int leftFrontSample[sampleSize];
int rightFrontSample[sampleSize];
int frontSample[frontSampleSize];
// Take a sample of measurements from the left, right and front
int i;
for (i = 0; i < sampleSize; i++) {
leftFrontSample[i] = get_front_ir_dist(LEFT);
rightFrontSample[i] = get_front_ir_dist(RIGHT);
usleep(500);
}
for (i = 0; i < frontSampleSize; i++) {
frontSample[i] = get_us_dist();
usleep(500);
}
// Take the median of those measurements to avoid misreads
int left = getMedian(leftFrontSample, sampleSize);
int front = getMedian(frontSample, frontSampleSize);
int right = getMedian(rightFrontSample, sampleSize);
adjacentNodes[0] = -1;
adjacentNodes[1] = -1;
adjacentNodes[2] = -1;
// No wall on the left
if (left >= 35) {
switch (orientation) {
case 0:
adjacentNodes[0] = currentNode - 1;
break;
case 1:
adjacentNodes[0] = currentNode + 4;
break;
case 2:
adjacentNodes[0] = currentNode + 1;
break;
case 3:
adjacentNodes[0] = currentNode - 4;
break;
}
}
// No wall in front
if (front >= 40) {
switch (orientation) {
case 0:
adjacentNodes[1] = currentNode + 4;
break;
case 1:
adjacentNodes[1] = currentNode + 1;
break;
case 2:
adjacentNodes[1] = currentNode - 4;
break;
case 3:
adjacentNodes[1] = currentNode - 1;
break;
}
}
// No wall on the right
if (right >= 35) {
switch (orientation) {
case 0:
adjacentNodes[2] = currentNode + 1;
break;
case 1:
adjacentNodes[2] = currentNode - 4;
break;
case 2:
adjacentNodes[2] = currentNode - 1;
break;
case 3:
adjacentNodes[2] = currentNode + 4;
break;
}
}
// Print what cells are on the left, right and front, and which of them are not walled off
printf("\n");
printf("Finding adjacent nodes \n");
printf("Left: %d; Front: %d; Right %d \n", left, front, right);
printf("Left: %d; Front: %d; Right %d \n", adjacentNodes[0], adjacentNodes[1], adjacentNodes[2]);
}
void correctSide(int currentNode, Location* currentLocation, int* orientation, Location mazeCoords[], int errorThreshold) {
printf("\n");
printf("Correct Side \n");
printf("current node: %d; orientation %d; \n", currentNode, *orientation);
printf("before correction: x = %f; y = %f \n", currentLocation->x, currentLocation->y);
int sampleSize = 20;
int sampleLeft[sampleSize];
int sampleRight[sampleSize];
int i;
for (i = 0; i < sampleSize; i++) {
sampleLeft[i] = get_front_ir_dist(LEFT);
sampleRight[i] = get_front_ir_dist(RIGHT);
usleep(500);
}
int measurementSL = getMedian(sampleLeft, sampleSize) - OFFSET_IR;
int measurementSR = getMedian(sampleRight, sampleSize) - OFFSET_IR;
double displacementFromCenterLeft = 0.0;
double displacementFromCenterRight = 0.0;
switch (*orientation) {
// North
case 0:
displacementFromCenterLeft = currentLocation->x - mazeCoords[currentNode].x;
break;
// East
case 1:
displacementFromCenterLeft = currentLocation->y - mazeCoords[currentNode].y;
break;
// South
case 2:
displacementFromCenterLeft = mazeCoords[currentNode].x - currentLocation->x;
break;
// West
case 3:
displacementFromCenterLeft = mazeCoords[currentNode].y - currentLocation->y;
break;
}
displacementFromCenterRight = -displacementFromCenterLeft;
double targetDisplacementLeft = 30.0 - (ROBOTWIDTH / 2.0) + displacementFromCenterLeft;
double targetDisplacementRight = 30.0 - (ROBOTWIDTH / 2.0) + displacementFromCenterRight;
printf("measurementLeft: %d; targetLeft: %f \n", measurementSL, targetDisplacementLeft);
printf("measurementRight: %d; targetRight %f \n", measurementSR, targetDisplacementRight);
// Only correct if error larger than this threshold
int leftError = targetDisplacementLeft - measurementSL;
int rightError = targetDisplacementRight - measurementSR;
int leftCorrection = leftError;
int rightCorrection = rightError;
// If there is a wall on one of the sides AND the error is large enough to be worth correcting
if (measurementSL < 30 && fabs(leftError) > errorThreshold) {
switch (*orientation) {
// North
case 0:
currentLocation->x -= leftCorrection;
break;
// East
case 1:
currentLocation->y += leftCorrection;
break;
// South
case 2:
currentLocation->x += leftCorrection;
break;
// West
case 3:
currentLocation->y -= leftCorrection;
break;
}
} else if (measurementSR < 30 && fabs(rightError) > errorThreshold) {
switch (*orientation) {
// North
case 0:
currentLocation->x += rightCorrection;
break;
// East
case 1:
currentLocation->y -= rightCorrection;
break;
// South
case 2:
currentLocation->x -= rightCorrection;
break;
// West
case 3:
currentLocation->y += rightCorrection;
break;
}
} else {
printf("no correction: x = %f; y = %f \n \n", currentLocation->x, currentLocation->y);
return;
}
printf("after correction: x = %f; y = %f \n \n", currentLocation->x, currentLocation->y);
}
