revise
Dependencies: HCSR04 PID PololuQik2 QEI mbed-rtos Sharp
Diff: main.cpp
- Revision:
- 14:3c8c4efe4786
- Parent:
- 13:9bad7f74833a
- Child:
- 15:a467af795e57
diff -r 9bad7f74833a -r 3c8c4efe4786 main.cpp --- a/main.cpp Sun Mar 30 00:29:49 2014 +0000 +++ b/main.cpp Sun Mar 30 18:21:06 2014 +0000 @@ -7,7 +7,7 @@ #include "stdio.h" #include "LPC17xx.h" #include "Sharp.h" - + #define PIN_TRIGGERL (p12) #define PIN_ECHOL (p11) #define PIN_TRIGGERR (p29) @@ -26,13 +26,15 @@ #define TOOLS (0) #define MID (1) #define RIGS (2) +#define MID2 (3) +#define RETURN (4) #define FAR (1) - - + + float range, range2, pid_return; void errFunction(void); bool cRc; - + //Hardware Initialization Serial bt(p13,p14); Serial pc(USBTX,USBRX); @@ -44,14 +46,15 @@ QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING); Sharp IR(p20); //InterruptIn encoder(p29); - - + + //Functions - + void wall_follow(int side, int direction, int section); void wall_follow2(int side, int direction, int section, float location, int rig); void leftTurn(void); void slightleft(void); +void slightright(void); void rightTurn(void); void slightMove(int direction, float pulses); void us_distance(void); @@ -59,54 +62,66 @@ void mid_section(float* location, float ¤t, int* direction); void mid_section2(float* location, float ¤t, int* direction); void rig_section(float* location, float ¤t, int* direction, int rig); +void tools_section_return(float* location, float ¤t); +void mid_section_return(float* location, float ¤t, int* direction); +void mid_section2_return(float* location, float ¤t, int* direction); +void rig_section_return(float* location, float ¤t, int* direction); void overBump(int section); void alignWithWall(int section); void UntilWall(int dir); - + //Variables - + int main(void) { float location[3], current=4; int direction[3]; double distance; - + pc.baud(115200); pc.baud(115200); motors.begin(); - + pc.printf("START\r\n"); //Go to tools tools_section(location, current); mid_section(location, current, direction); mid_section2(location, current, direction); rig_section(location, current, direction, 3); + // fire putting out + wait(2); + // + rig_section_return(location, current, direction); + mid_section2_return(location, current, direction); + mid_section_return(location, current, direction); + tools_section_return(location,current); + /*while(1) { pc.printf("IR %f\r\n", IR.getDistance()); /*rangeFinderLeft.startMeas(); rangeFinderRight.startMeas(); wait_ms(38); rangeFinderLeft.getMeas(range); - rangeFinderRight.getMeas(range); + rangeFinderRight.getMeas(range); pc.printf("leftRange = %f\n\r", range); pc.printf("rightRange = %f\n\r", range); - + wait_ms(200); }*/ - - //wall_follow2(LEFT,FORWARD,MID,0); + +//wall_follow2(LEFT,FORWARD,MID,0); //leftTurn(); //rightTurn(); - - + + } - + void errFunction(void) { //Nothing } - + void us_distance(void) { pc.printf("Ultra Sonic\n\r"); @@ -116,25 +131,25 @@ pc.printf("Range = %f\n\r", range); } } - + void wall_follow(int side, int direction, int section) { float location, set=6; int dir=1; pid1.reset(); - + if(direction == BACKWARD) dir=-1; if(section == TOOLS)set= 10; - + leftEncoder.reset(); rightEncoder.reset(); - + location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - + while(location< 69) { location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - + pid1.setInputLimits(0, set); pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); pid1.setSetPoint(set); @@ -148,17 +163,17 @@ rangeFinderRight.getMeas(range); pc.printf("%d\r\n",range); } - - if(range > 20) { + + if(range > 15) { //pc.printf("wavegap %f\r\n",wavegap); // AT WAVE OPENING!!!! motors.setMotor1Speed(dir*0.25*127);//left motors.setMotor0Speed(dir*0.25*127);//right } else { - + pid1.setProcessValue(range); pid_return = pid1.compute(); - + if(pid_return > 0) { if(side) { motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right @@ -182,62 +197,65 @@ } } } - + /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ - + void wall_follow2(int side, int direction, int section, float location, int rig) { - int dir=1, limit=90; + int dir=1, limit=86, lowlim=5; float set=6, loc=0, Rigloc=0; - + bool SeeWaveGap = false; + if(rig == 1) Rigloc= 16; else if(rig == 2) Rigloc= 45; else if(rig== 3) Rigloc = 70; - + pid1.reset(); - - if(direction == BACKWARD){ - dir=-1; - limit = 100; + + if(direction == BACKWARD) { + dir=-1; + limit = 100; } - if(section == TOOLS){ + if(section == TOOLS) { set= 6; - limit = 89; + limit = 86; } + if(section == RETURN) lowlim=15; leftEncoder.reset(); rightEncoder.reset(); - + //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); - - while((dir*loc + location <= limit) && (dir*loc + location >= 5)) { + + while((dir*loc + location <= limit) && (dir*loc + location >= lowlim)) { + loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; pc.printf("loc %f \r\n", loc); - + pid1.setInputLimits(0.0, set); pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); pid1.setSetPoint(set); - + if(side) { rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); - } else{ + } else { rangeFinderRight.startMeas(); wait_ms(20); rangeFinderRight.getMeas(range); } - - if(section == RIGS){ + + if(section == RIGS) { rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range2); - - if(range2< 20){ - if( abs(dir*loc + location - Rigloc) < 10){ + + 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 @@ -247,66 +265,70 @@ } } } - - + + //pc.printf("wall follow 2 range %f\r\n",range); //pc.printf("loc+location = %f\r\n", loc+location); if(range > 20 ) { - if(section == RIGS){ + if(section == RIGS || section == RETURN) { 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 { + if(!SeeWaveGap) { + SeeWaveGap=true; + }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); - pid_return = pid1.compute(); - //pc.printf("Range: %f\n PID: %f\r\n", range, pid_return); - - if(pid_return > 0) { - if(side) { - motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right - motors.setMotor1Speed(dir*MAX_SPEED);//left + SeeWaveGap = false; + pid1.setProcessValue(range); + pid_return = pid1.compute(); + //pc.printf("Range: %f\n PID: %f\r\n", range, pid_return); + + if(pid_return > 0) { + if(side) { + motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right + motors.setMotor1Speed(dir*MAX_SPEED);//left + } else { + motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left + motors.setMotor0Speed(dir*MAX_SPEED);//right + } + } else if(pid_return < 0) { + if(side) { + motors.setMotor0Speed(dir*MAX_SPEED);//right + motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left + } else { + motors.setMotor1Speed(dir*MAX_SPEED);//left + motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right + } } else { - motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left - motors.setMotor0Speed(dir*MAX_SPEED);//right + motors.setMotor0Speed(dir*MAX_SPEED); + motors.setMotor1Speed(dir*MAX_SPEED); } - } else if(pid_return < 0) { - if(side) { - motors.setMotor0Speed(dir*MAX_SPEED);//right - motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left - } else { - motors.setMotor1Speed(dir*MAX_SPEED);//left - motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right - } - } else { - motors.setMotor0Speed(dir*MAX_SPEED); - motors.setMotor1Speed(dir*MAX_SPEED); } - }} - + } + //STOP - motors.setMotor0Speed(dir*-0.25*127); //right - motors.setMotor1Speed(dir*-0.25*127); //left + motors.setMotor0Speed(dir*-0.3*127); //right + motors.setMotor1Speed(dir*-0.3*127); //left wait_ms(5); motors.stopBothMotors(); } - - + + void alignWithWall(int section) { float usValue = 0; - + if(section == TOOLS) { pc.printf("tools section align\r\n"); // turn at an angle @@ -316,7 +338,7 @@ motors.setMotor1Speed(0.4*MAX_SPEED); //left while(rightEncoder.getPulses()>-1000); motors.stopBothMotors(); - + //go backwards toward wall leftEncoder.reset(); rightEncoder.reset(); @@ -324,31 +346,60 @@ motors.setMotor1Speed(-0.25*127); //left while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300); motors.stopBothMotors(); - + // turn left towards wall leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(MAX_SPEED); //right motors.setMotor1Speed(-MAX_SPEED); //left - while(rightEncoder.getPulses() < 100 || abs(leftEncoder.getPulses()) < 100); - + while(rightEncoder.getPulses() < 20 || abs(leftEncoder.getPulses()) < 20); + motors.stopBothMotors(); - + // turning left motors.setMotor0Speed(0.9*MAX_SPEED); //right motors.setMotor1Speed(-0.9*MAX_SPEED); //left - - }else if( section == RIGS){ - rightTurn(); - // turning right towards wall + + } else if( section == RIGS) { + // check distance to wall + rangeFinderRight.startMeas(); + wait_ms(20); + rangeFinderRight.getMeas(range); + + if(range < 4 || range > 20) return; + + // turn at an angle + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor1Speed(-1.2*MAX_SPEED); //left + motors.setMotor0Speed(0.4*MAX_SPEED); //right + while(abs(leftEncoder.getPulses())<1000); + motors.stopBothMotors(); + + //go backwards toward wall + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(-0.25*127); //right + motors.setMotor1Speed(-0.25*127); //left + while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150); + motors.stopBothMotors(); + + // turn left towards wall + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(-MAX_SPEED); //right + motors.setMotor1Speed(MAX_SPEED); //left + while(abs(rightEncoder.getPulses()) < 20 || abs(leftEncoder.getPulses()) < 20); + + motors.stopBothMotors(); + + // turning left motors.setMotor0Speed(-0.9*MAX_SPEED); //right motors.setMotor1Speed(0.9*MAX_SPEED); //left - } - else { + } else { pc.printf("in mid section align\r\n"); // turn right towards wall rightTurn(); - // turning left towards wall motors.setMotor0Speed(0.9*MAX_SPEED); //right motors.setMotor1Speed(-0.9*MAX_SPEED); //left @@ -356,12 +407,11 @@ usValue = 0; while(1) { - if(section == RIGS){ + if(section == RIGS) { rangeFinderRight.startMeas(); wait_ms(20); rangeFinderRight.getMeas(range); - } - else{ + } else { rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); @@ -375,7 +425,7 @@ } motors.stopBothMotors(); } - + void rightTurn(void) { motors.begin(); @@ -386,7 +436,7 @@ while(abs(leftEncoder.getPulses())<950 || abs(rightEncoder.getPulses())<950); motors.stopBothMotors(); } - + void leftTurn(void) { motors.begin(); @@ -397,9 +447,10 @@ while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100); motors.stopBothMotors(); } - -void slightleft(void){ - + +void slightleft(void) +{ + leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(0.5*127);// right @@ -407,178 +458,215 @@ while(abs(leftEncoder.getPulses())<90 || rightEncoder.getPulses()<90); motors.stopBothMotors(); } -void slightMove(int direction, float pulses){ + +void slightright(void) +{ + + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(-0.4*127);// right + motors.setMotor1Speed(0.4*127);// left + while(abs(leftEncoder.getPulses())<90 || abs(rightEncoder.getPulses())<90); + motors.stopBothMotors(); +} + +void slightMove(int direction, float pulses) +{ int dir=1; - + if(direction == BACKWARD) dir= -1; - + leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(dir*0.25*127); //right motors.setMotor1Speed(dir*0.25*127); //left while(abs(leftEncoder.getPulses()) < pulses || abs(rightEncoder.getPulses()) < pulses); - + motors.setMotor0Speed(dir*-0.25*127); //right motors.setMotor1Speed(dir*-0.25*127); //left wait_ms(10); motors.stopBothMotors(); } -void UntilWall(int dir){ - - if(dir == BACKWARD) dir=-1; - +void UntilWall(int dir) +{ + + if(dir == BACKWARD) dir=-1; + leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(dir*0.2*127); //right motors.setMotor1Speed(dir*0.2*127); //left - + range = 30; - - while(range > 20){ + + while(range > 20) { rangeFinderRight.startMeas(); wait_ms(20); rangeFinderRight.getMeas(range); } - + motors.setMotor0Speed(dir*-0.2*127); //right motors.setMotor1Speed(dir*-0.2*127); //left wait_ms(5); motors.stopBothMotors(); -} - +} + void overBump(int section) { int preLeft=5000, preRight=5000, out=0; - + motors.begin(); - // slight backwards + // slight backwards leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-0.25*127); //right motors.setMotor1Speed(-0.25*127); //left while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50); motors.stopBothMotors(); - + pc.printf("slight backwards\r\n"); wait_ms(200); - - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(0.35*127); //right - motors.setMotor1Speed(0.35*127); //left - while((abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses()) < 1000) && preLeft!=0 && IR.getDistance() >15 ){ - preLeft=leftEncoder.getPulses(); - preRight=rightEncoder.getPulses(); - wait_ms(200); - if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0; - } - - pc.printf("forward \r\n"); - wait_ms(200); - /* - motors.stopBothMotors(); - motors.begin(); - + leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(0.3*127); //right motors.setMotor1Speed(0.3*127); //left - - while(!out) { - preLeft=leftEncoder.getPulses(); + while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getDistance() >15 ) { + /*preLeft=leftEncoder.getPulses(); preRight=rightEncoder.getPulses(); - - rangeFinderLeft.startMeas(); - rangeFinderRight.startMeas(); - wait_ms(20); - rangeFinderLeft.getMeas(range); - rangeFinderRight.getMeas(range2); - if(range < 10 || range2 < 10) out=1; - - if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) { - motors.setMotor0Speed(0.4*127); //right - motors.setMotor1Speed(0.4*127); //left - wait_ms(50); - out=1; - } - if(abs(leftEncoder.getPulses()) >1000 || abs(leftEncoder.getPulses())>1000) out=1; + wait_ms(200); + if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;*/ } - */ - + + pc.printf("forward \r\n"); + wait_ms(200); + /* + motors.stopBothMotors(); + motors.begin(); + + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(0.3*127); //right + motors.setMotor1Speed(0.3*127); //left + + while(!out) { + preLeft=leftEncoder.getPulses(); + preRight=rightEncoder.getPulses(); + + rangeFinderLeft.startMeas(); + rangeFinderRight.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range); + rangeFinderRight.getMeas(range2); + if(range < 10 || range2 < 10) out=1; + + if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) { + motors.setMotor0Speed(0.4*127); //right + motors.setMotor1Speed(0.4*127); //left + wait_ms(50); + out=1; + } + if(abs(leftEncoder.getPulses()) >1000 || abs(leftEncoder.getPulses())>1000) out=1; + } + */ + motors.stopBothMotors(); motors.begin(); - + preLeft=preRight=5000 ; leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(.25*127); //right motors.setMotor1Speed(.25*127); //left - - if(section == TOOLS){ - while(IR.getDistance() > 10 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)){ - + + if(section == TOOLS) { + while(IR.getDistance() > 10 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) { + if(IR.getDistance() > 38) break; - + preLeft=leftEncoder.getPulses(); preRight=rightEncoder.getPulses(); wait_ms(200); } - } - else if( section == MID ){ - while(IR.getDistance() > 15 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)){ - + } else if(section == MID || section == MID2) { + if(section == MID2) while(IR.getDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400)); + while(IR.getDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) { + if(IR.getDistance() > 38) break; - + preLeft=leftEncoder.getPulses(); preRight=rightEncoder.getPulses(); wait_ms(200); } - + + } else { + while(abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100); + + leftEncoder.reset(); + rightEncoder.reset(); + + motors.setMotor0Speed(-.15*127); //right + motors.setMotor1Speed(-.15*127); //left + while((abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && preLeft!=0 ) { + preLeft = leftEncoder.getPulses(); + preRight = rightEncoder.getPulses(); + wait_ms(200); + if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0; + } + + leftEncoder.reset(); + rightEncoder.reset(); + + motors.setMotor0Speed(0.25*127); //right + motors.setMotor1Speed(0.25*127); //left + while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10)); + + motors.stopBothMotors(); + + return; } - else while(abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100); - - motors.stopBothMotors(); - + leftEncoder.reset(); rightEncoder.reset(); - + motors.setMotor0Speed(-.25*127); //right motors.setMotor1Speed(-.25*127); //left while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10)); - + motors.stopBothMotors(); - wait_ms(50); + wait_ms(20); motors.begin(); - + } - + void tools_section(float* location, float ¤t) { wall_follow(LEFT,FORWARD, TOOLS); + //HARD STOP motors.setMotor0Speed(-.3*127); //right motors.setMotor1Speed(-.3*127); //left - wait_ms(10); + wait_ms(5); motors.stopBothMotors(); // current position in reference to the starting position current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; pc.printf("current %f \r\n",current); - + //Tool aquiring wait(2); // After tool is aquired - + alignWithWall(TOOLS); pc.printf("align\r\n"); wait_ms(100); - + //wall_follow2(LEFT,FORWARD,MID, current); //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - + rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); - + if(range < 20) { wall_follow2(LEFT,BACKWARD,TOOLS, current,0); pc.printf("wall follow\r\n"); @@ -598,7 +686,7 @@ motors.setMotor1Speed(MAX_SPEED); //left while(abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses())< 10); motors.stopBothMotors(); - + wait_ms(100); leftTurn(); overBump(TOOLS); @@ -608,15 +696,15 @@ leftTurn(); overBump(TOOLS); } - + pc.printf("First Wavegap = %f\r\n",location[0]); } - + void mid_section(float* location, float ¤t, int* direction) { motors.begin(); - - if(IR.getDistance() > 38){ + + if(IR.getDistance() > 38) { direction[0]= STRAIGHT; overBump(MID); return; @@ -624,72 +712,74 @@ pc.printf("before align with wall \r\n"); alignWithWall(MID); wait_ms(100); - + pc.printf("mid section current = %f\r\n",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); - + wait_ms(500); rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); - + if(range > 20 ) { direction[0]= RIGHT; location[1]= current; - //slightMove(FORWARD,200); - current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + slightMove(FORWARD,75); + //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } else { direction[0]= LEFT; 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){ + + if(location[1] < 18) { slightMove(FORWARD, 50); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - } - + } + } - + pc.printf("wavegap2 = %f\r\n",location[1]); leftTurn(); - + wait_ms(100); - + overBump(MID); - + } - + void mid_section2(float* location, float ¤t, int* direction) { motors.begin(); - + pc.printf("mid section 2\r\n"); - - if(IR.getDistance() > 38){ - direction[0]= STRAIGHT; + + if(IR.getDistance() > 38) { + direction[1]= STRAIGHT; overBump(RIGS); return; } alignWithWall(MID); pc.printf("midsection 2 alignt with wall mid \r\n"); - + 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"); + + wait_ms(500); + + pc.printf("midseection 2 after wf2 %f",current); rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); - + if(range > 20 ) { direction[1]= RIGHT; location[2]= current; - slightMove(FORWARD,500); - current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + slightMove(FORWARD,75); + //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } else { direction[1]= LEFT; wall_follow2(LEFT,BACKWARD,MID,current,0); @@ -697,30 +787,89 @@ current=location[2]; //slightMove(FORWARD,500); } - + leftTurn(); overBump(RIGS); pc.printf("overbump rigs\r\n"); } - + void rig_section(float* location, float ¤t, int* direction, int rig) { float loc; - + if(rig == 1) loc= 15; else if(rig == 2) loc= 45; else loc = 75; - + rightTurn(); - - if(current > loc){ + slightright(); + + if(current > loc) { 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); - } - else{ + } else { 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); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + } +} + +void tools_section_return(float* location, float ¤t) +{ + if(location[0] > 16) { + leftTurn(); + wall_follow2(LEFT, BACKWARD, RETURN, location[0], 0); + } + motors.stopBothMotors(); + +} + +void mid_section_return(float* location, float ¤t, int* direction) +{ + if(direction[0] == RIGHT) { + leftTurn(); + alignWithWall(MID); + wall_follow2(LEFT, BACKWARD, MID, current,0); + current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + rightTurn(); + } else if(direction[0] == LEFT) { + leftTurn(); + wall_follow2(RIGHT, FORWARD, MID, current,0); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + rightTurn(); } + //ELSE and GO FORWARD + overBump(RIGS); +} + +void mid_section2_return(float* location, float ¤t, int* direction) +{ + if(direction[1] == RIGHT) { + leftTurn(); + wall_follow2(LEFT, BACKWARD, MID, current,0); + current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + rightTurn(); + } else if(direction[1] == LEFT) { + leftTurn(); + wall_follow2(RIGHT, FORWARD, MID, current,0); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + rightTurn(); + } + //ELSE and GO FORWARD + overBump(MID); +} + +void rig_section_return(float* location, float ¤t, int* direction) +{ + alignWithWall(RIGS); + if(location[2] > current) { + wall_follow2(RIGHT, FORWARD, MID, current,0); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + } else { + wall_follow2(RIGHT, BACKWARD, MID, current,0); + current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + } + rightTurn(); + overBump(MID2); } \ No newline at end of file