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