revise
Dependencies: HCSR04 PID PololuQik2 QEI mbed-rtos Sharp
Diff: main.cpp
- Revision:
- 13:9bad7f74833a
- Parent:
- 12:168cb595f98e
- Child:
- 14:3c8c4efe4786
--- a/main.cpp Sat Mar 29 22:20:44 2014 +0000 +++ b/main.cpp Sun Mar 30 00:29:49 2014 +0000 @@ -49,8 +49,7 @@ //Functions void wall_follow(int side, int direction, int section); -void wall_follow2(int side, int direction, int section, float location); -void wall_follow3(int ¤tLocation, int &WaveOpening); +void wall_follow2(int side, int direction, int section, float location, int rig); void leftTurn(void); void slightleft(void); void rightTurn(void); @@ -82,7 +81,7 @@ tools_section(location, current); mid_section(location, current, direction); mid_section2(location, current, direction); - rig_section(location, current, direction, 1); + rig_section(location, current, direction, 3); /*while(1) { pc.printf("IR %f\r\n", IR.getDistance()); /*rangeFinderLeft.startMeas(); @@ -186,10 +185,14 @@ /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ -void wall_follow2(int side, int direction, int section, float location) +void wall_follow2(int side, int direction, int section, float location, int rig) { - int dir=1, limit=89; - float set=6, loc=0; + int dir=1, limit=90; + float set=6, loc=0, Rigloc=0; + + if(rig == 1) Rigloc= 16; + else if(rig == 2) Rigloc= 45; + else if(rig== 3) Rigloc = 70; pid1.reset(); @@ -205,7 +208,7 @@ leftEncoder.reset(); rightEncoder.reset(); - pc.printf("before %f\r\n", location); + //pc.printf("before %f\r\n", location); pc.printf("dir*loc+location %f\r\n",dir*loc + location ); pc.printf("limit %d \r\n", limit); @@ -229,12 +232,19 @@ } if(section == RIGS){ - rangeFinderRight.startMeas(); + rangeFinderLeft.startMeas(); wait_ms(20); - rangeFinderRight.getMeas(range); - if(range< 20){ - motors.stopBothMotors(); - break; + rangeFinderLeft.getMeas(range2); + + if(range2< 20){ + if( abs(dir*loc + location - Rigloc) < 10){ + //STOP + motors.setMotor0Speed(dir*-0.25*127); //right + motors.setMotor1Speed(dir*-0.25*127); //left + wait_ms(5); + motors.stopBothMotors(); + break; + } } } @@ -242,10 +252,21 @@ //pc.printf("wall follow 2 range %f\r\n",range); //pc.printf("loc+location = %f\r\n", loc+location); if(range > 20 ) { - motors.stopBothMotors(); - pc.printf("wavegap\r\n"); - // AT WAVE OPENING!!!! - break; + if(section == RIGS){ + motors.setMotor0Speed(dir*0.25*127); //right + motors.setMotor1Speed(dir*0.25*127); //left + } + else{ + //STOP + motors.setMotor0Speed(dir*-0.25*127); //right + motors.setMotor1Speed(dir*-0.25*127); //left + wait_ms(5); + motors.stopBothMotors(); + + pc.printf("wavegap\r\n"); + // AT WAVE OPENING!!!! + break; + } } else { pid1.setProcessValue(range); @@ -274,6 +295,7 @@ } }} + //STOP motors.setMotor0Speed(dir*-0.25*127); //right motors.setMotor1Speed(dir*-0.25*127); //left wait_ms(5); @@ -396,11 +418,9 @@ motors.setMotor1Speed(dir*0.25*127); //left while(abs(leftEncoder.getPulses()) < pulses || abs(rightEncoder.getPulses()) < pulses); - leftEncoder.reset(); - rightEncoder.reset(); motors.setMotor0Speed(dir*-0.25*127); //right motors.setMotor1Speed(dir*-0.25*127); //left - while(abs(leftEncoder.getPulses()) < 20 || abs(rightEncoder.getPulses()) < 20); + wait_ms(10); motors.stopBothMotors(); } @@ -560,7 +580,7 @@ rangeFinderLeft.getMeas(range); if(range < 20) { - wall_follow2(LEFT,BACKWARD,TOOLS, current); + wall_follow2(LEFT,BACKWARD,TOOLS, current,0); pc.printf("wall follow\r\n"); location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); current= location[0]; @@ -606,7 +626,7 @@ wait_ms(100); pc.printf("mid section current = %f\r\n",current); - wall_follow2(LEFT,FORWARD,MID, current); + wall_follow2(LEFT,FORWARD,MID, current,0); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); pc.printf("after wf2 current = %f\r\n",current); @@ -618,14 +638,17 @@ if(range > 20 ) { direction[0]= RIGHT; location[1]= current; - slightMove(FORWARD,100); + //slightMove(FORWARD,200); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } else { direction[0]= LEFT; - wall_follow2(LEFT,BACKWARD,MID,current); + wall_follow2(LEFT,BACKWARD,MID,current,0); location[1]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); current= location[1]; + if(location[1] < 18){ slightMove(FORWARD, 50); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } } @@ -654,7 +677,7 @@ alignWithWall(MID); pc.printf("midsection 2 alignt with wall mid \r\n"); - wall_follow2(LEFT,FORWARD,MID, current); + wall_follow2(LEFT,FORWARD,MID, current,0); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); pc.printf("midseection 2 after wf2"); @@ -666,12 +689,13 @@ direction[1]= RIGHT; location[2]= current; slightMove(FORWARD,500); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } else { direction[1]= LEFT; - wall_follow2(LEFT,BACKWARD,MID,current); + wall_follow2(LEFT,BACKWARD,MID,current,0); location[2]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); current=location[2]; - slightMove(BACKWARD,100); + //slightMove(FORWARD,500); } leftTurn(); @@ -683,33 +707,20 @@ { float loc; - if(rig == 1) loc= 16; + if(rig == 1) loc= 15; else if(rig == 2) loc= 45; - else loc = 70; + else loc = 75; - alignWithWall(RIGS); + rightTurn(); if(current > loc){ - UntilWall(BACKWARD); - current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - wall_follow2(RIGHT, BACKWARD, RIGS, current); + pc.printf("RIG section %f\r\n",current); + wall_follow2(RIGHT, BACKWARD, RIGS, current, rig); current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - - if((current- loc)>10){ - wall_follow2(RIGHT, BACKWARD, RIGS, current); - current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - } } else{ - UntilWall(FORWARD); - wall_follow2(RIGHT, FORWARD, RIGS, current); - current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - - if((current- loc)<-10){ - wall_follow2(RIGHT, FORWARD, RIGS, current); - current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - } + pc.printf("RIG section %f\r\n",current); + wall_follow2(RIGHT, FORWARD, RIGS, current, rig); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } - - } \ No newline at end of file