// Basic Left wall following code void wallFollowingL() { int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i; while (1) { get_front_ir_dists(&leftFrontIR, &rightFrontIR); get_side_ir_dists(&leftSideIR, &rightSideIR); distAhead = get_us_dist(); if (distAhead < 40){ if (leftFrontIR - rightFrontIR > 0){ set_motors(-SPEED, SPEED); } else { set_motors(SPEED, -SPEED); } } if (leftFrontIR > 30 ) { set_motors(SPEED-10, SPEED+10); } //turn left else if (leftFrontIR < 15 ) { set_motors (SPEED+10, SPEED-10); } //turn right else {set_motors(SPEED, SPEED); } // Straight } }
// Basic Left wall following code void wallFollowingL() { int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i; while (1) //for(i = 0; i < 250; i++) { get_front_ir_dists(&leftFrontIR, &rightFrontIR); get_side_ir_dists(&leftSideIR, &rightSideIR); distAhead = get_us_dist(); printf("i = %i\n", i); if ( rightFrontIR < 30 ) { turnRight (90.0, 5);} // super turn right else if (leftFrontIR > 30 && leftSideIR > 25) { set_motors(SPEED-15, SPEED+50);} // super turn left else if (leftFrontIR > 38) { set_motors(SPEED, 30);} // too far, turn left else if ( leftFrontIR < 18 ) { set_motors(SPEED*1.5, SPEED);} // turn right else {set_motors(SPEED+5, SPEED+5); } // Straight } }
// Basic Right wall following code void wallFollowingR() { int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i; while (1) { get_front_ir_dists(&leftFrontIR, &rightFrontIR); get_side_ir_dists(&leftSideIR, &rightSideIR); distAhead = get_us_dist(); if (distAhead < 45){ if (leftFrontIR - rightFrontIR > 0){ set_motors(-SPEED, SPEED); } else { set_motors(20, -20); } } else if (rightFrontIR > 30 && rightSideIR > 25) { set_motors(SPEED+30, SPEED-8); } //turn right else if (rightFrontIR < 20 ) { set_motors (SPEED-10, SPEED+25); } //turn left else {set_motors(SPEED+8, SPEED+8); } // Straight } }
// Basic Right wall following code void wallFollowingR() { int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i; while (1) //for(i = 0; i < 170; i++) { if (thetaAcc > 40) { break; } get_front_ir_dists(&leftFrontIR, &rightFrontIR); get_side_ir_dists(&leftSideIR, &rightSideIR); distAhead = get_us_dist(); int lbump, rbump; check_bumpers(&lbump, &rbump); if (lbump){ set_motors(SPEED+20, SPEED-20); } if (rbump){ set_motors(SPEED-20, SPEED+20); } if (distAhead < 35){ //in the L shape if (leftFrontIR - rightFrontIR > 0){ set_motors(-SPEED, SPEED); } else { set_motors(+20,-20); } } else if (rightFrontIR > 30) { set_motors(SPEED+10, SPEED-20); } //turn right else if (rightFrontIR < 20) { set_motors (SPEED-25, SPEED+15); } //turn left else {set_motors(SPEED+7, SPEED+7); } // Straight calculateTheta(); } }
char* getPath(int startx, int starty, int destx, int desty) { //moves the robot from a square to another (in the grid where start = (0, 0)), following left wall, and records the path taken. int left, right, index = 0, currentx = startx, currenty = starty; //stores current position of robot in the grid in currentx and currenty int idealBearing = getIdealBearing(); char* path = malloc(sizeof(char) * PATH_MAX_LENGTH); while (currentx != destx || currenty != desty) { get_front_ir_dists(&left, &right); int front = get_us_dist(); /*if(front < MAX_DIST_TO_WALL) { checkFrontWall(front); }*/ //checks accurancy of the robot's calculated current position vs its real position log_trail(); set_point(xpos, ypos); if(left > MAX_DIST_TO_WALL) { //turn left turnLeft(PHASE1_SPEED, PHASE1_SLOW, 90); idealBearing -= 90; if(idealBearing < 0) { idealBearing += 360; } path[index] = 'L'; } else if(front > MAX_DIST_TO_WALL) { //go straight path[index] = 'S'; } else if(right > MAX_DIST_TO_WALL) { //turn right turnRight(PHASE1_SPEED, PHASE1_SLOW, 90); idealBearing += 90; if(idealBearing >= 360) { idealBearing -= 360; } path[index] = 'R'; } else { //turn around turnRight(PHASE1_SPEED, PHASE1_SLOW, 180); idealBearing += 180; if(idealBearing >= 360) { idealBearing -= 360; } path[index] = 'B'; } fixBearing(idealBearing); moveForwards(PHASE1_SPEED, PHASE1_SLOW, SQUARE_DIST * CM_TO_ENCODER); //uses robot's idealBearing to figure out which square in the grid the robot is currently in after moving switch(idealBearing) { case 0: currenty++; break; case 90: currentx++; break; case 180: currenty--; break; case 270: currentx--; break; default: printf("Error: unexpected value for idealBearing\n"); break; } index++; } path[index] = '\0'; return path; }
//Basic wall following code void wallFollowing() { //Sensor readings int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR; int ultrasound; get_front_ir_dists(&leftFrontIR, &rightFrontIR); get_side_ir_dists(&leftSideIR, &rightSideIR); inputMotors(); while (1) { if (thetaAcc > 40) { break; } int lbump, rbump; check_bumpers(&lbump, &rbump); if (lbump){ set_motors(SPEED+20, SPEED-20); } if (rbump){ set_motors(SPEED-20, SPEED+20); } get_front_ir_dists(&leftFrontIR, &rightFrontIR); get_side_ir_dists(&leftSideIR, &rightSideIR); ultrasound = get_us_dist(); //Hit wall if (ultrasound < 30){ int i; /* for(i=0; i<20; i++){ set_motors(-SPEED, -SPEED); ultrasound = get_us_dist(); } */ //set_ir_angle(RIGHT, 45); //set_ir_angle(LEFT, -45); get_front_ir_dists(&leftFrontIR, &rightFrontIR); //for (i=0; i<15; i++){ if (leftFrontIR - rightFrontIR > 0){ set_motors(-SPEED, -SPEED); //set_motors(-SPEED, SPEED); set_motors(-25,25); } else{ set_motors(-SPEED, -SPEED); set_motors(SPEED, -SPEED); } get_front_ir_dists(&leftFrontIR, &rightFrontIR); //} setAngles(); } /* //Deadlock if (ultrasound < 30 && leftFrontIR < 40 && rightFrontIR < 40) { int i; //while ((ultrasound < 80 && leftFrontIR < 45) || (ultrasound < 80 && rightFrontIR < 45)) while (ultrasound < 87) { set_motors(-SPEED, -SPEED); //get_side_ir_dists(&leftFrontIR, &rightSideIR); ultrasound = get_us_dist(); } set_ir_angle(RIGHT, 45); set_ir_angle(LEFT, -45); get_front_ir_dists(&leftFrontIR, &rightFrontIR); //printf("%d %d\n", leftFrontIR, rightFrontIR); if (leftFrontIR - rightFrontIR > 0){ //for (i=0; i<20; i++) set_motors(-SPEED-20, -SPEED+20); //turnLeft(50); } else if (rightFrontIR - leftFrontIR > 0){ //for (i=0; i<20; i++) set_motors(-SPEED+20, -SPEED-20); //turnRight(50); } setAngles(); //Make 90 deg left turn //for (i=0; i<35; i++) //{ set_motors(SPEED, -SPEED); } } */ else{ if (leftFrontIR > rightFrontIR) { set_motors(SPEED-25, SPEED+25); //addNode(pointer, LEFT); } else if (rightFrontIR > leftFrontIR ) { set_motors(SPEED+25, SPEED-25); //addNode(pointer, RIGHT); } else { set_motors(SPEED, SPEED); //addNode(pointer, STRAIGHT); } } calculateTheta(); } }
void getIRDist(){ ultrasound = get_us_dist(); get_side_ir_dists(&leftSideIR, &rightSideIR); get_front_ir_dists(&leftFrontIR, &rightFrontIR); }