Tools Section Navigation Calibrated slightly
Dependencies: HCSR04 PID PololuQik2 QEI mbed-rtos
Fork of NavigationTest by
Revision 8:32ba0fad1689, committed 2014-03-22
- Comitter:
- jjcarr2
- Date:
- Sat Mar 22 16:24:59 2014 +0000
- Parent:
- 7:0b7897232e93
- Commit message:
- Modular function blah;
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 0b7897232e93 -r 32ba0fad1689 main.cpp --- a/main.cpp Fri Mar 21 00:02:51 2014 +0000 +++ b/main.cpp Sat Mar 22 16:24:59 2014 +0000 @@ -6,7 +6,7 @@ #include "HCSR04.h" #include "stdio.h" #include "LPC17xx.h" - + #define PIN_TRIGGERL (p12) #define PIN_ECHOL (p11) #define PIN_TRIGGERR (p29) @@ -20,328 +20,233 @@ #define LEFT (1) #define RIGHT (0) #define FORWARD (1) -#define BACKWARD (0) +#define BACKWARD (0) #define TOOLS (0) #define MID (1) -#define RIGS (2) - -float range, pid_return; +#define FIRST_WAVE (0) +#define FAR (1) + + +float range, pid_return, usValue; void errFunction(void); bool cRc; - + //Hardware Initialization Serial bt(p13,p14); Serial pc(USBTX,USBRX); HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL ); HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR ); -PID pid1(15.0,0.0,10.0,0.02); +PID pid1(15.0,0.0,4.0,0.02); PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc); QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING); QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING); //InterruptIn encoder(p29); - - + + //Functions - + float wall_follow(int side, int direction, int section); -void wall_follow2(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 leftTurn(void); void rightTurn(void); -void us_distance(void); -void tools_section(float *location, float ¤t); -void overBump(void); -void alignWithWall(void); - +void us_distance(int side); +void tools_section(float* location, float ¤t); +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 overBump(int wave); +void alignWithWall(int section); +void alignParallel(int side, int turn, float limit); + //Variables - + int main(void) { float location[3], current=0; int direction[3]; - + pc.baud(115200); bt.baud(115200); motors.begin(); - - /*****************************OLD CODE FOR GOING THROUGH WAVE SECTION****************/ - /* - leftEncoder.reset(); - rightEncoder.reset(); - wall_follow2(LEFT,FORWARD,TOOLS); - - motors.stopBothMotors(); - wait_ms(500); - leftEncoder.reset(); - rightEncoder.reset(); - - motors.setMotor0Speed(0.6*MAX_SPEED); //right - motors.setMotor1Speed(0.6*MAX_SPEED); //left - - wait_ms(200); - motors.stopBothMotors(); - - leftEncoder.reset(); - rightEncoder.reset(); - bt.printf("1st turn left\r\n"); - - motors.setMotor0Speed(MAX_SPEED); //right - motors.setMotor1Speed(-MAX_SPEED); //left - - while(((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2) < 1); - - leftEncoder.reset(); - rightEncoder.reset(); - - motors.setMotor0Speed(MAX_SPEED); //right - motors.setMotor1Speed(MAX_SPEED); //left - - while(((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/PPR)/2) < 2); - - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-MAX_SPEED); //right - motors.setMotor1Speed(MAX_SPEED); //left - float usValue = 0; - while(1){ - rangeFinderLeft.startMeas(); - wait_ms(100); - rangeFinderLeft.getMeas(range); - bt.printf("Range %f \t OldValue %f\n\r",range, usValue); - if(range > usValue && usValue != 0 && range < 10){ - break; - } else { - usValue = range; - } - } - motors.stopBothMotors(); - - leftEncoder.reset(); - rightEncoder.reset(); - - wall_follow2(LEFT,FORWARD,TOOLS); - - while(1){} - */ - /* - // Very Consistent Turn - leftEncoder.reset(); - rightEncoder.reset(); - - motors.setMotor0Speed(-MAX_SPEED); //right - motors.setMotor1Speed(MAX_SPEED); //left - float usValue = 0; - while(1){ - rangeFinderLeft.startMeas(); - wait_ms(100); - rangeFinderLeft.getMeas(range); - bt.printf("Range %f \t OldValue %f\n\r",range, usValue); - if(range > usValue && usValue != 0 && range < 10){ - break; - } else { - usValue = range; - } - } - */ - motors.stopBothMotors(); - /* - - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(MAX_SPEED); //right - motors.setMotor1Speed(MAX_SPEED); //left - while(leftEncoder.getPulses()/(PPR) < 3); - */ - - //leftEncoder.reset(); - //rightEncoder.reset(); - //motors.setMotor0Speed(MAX_SPEED); //right - //motors.setMotor1Speed(MAX_SPEED); //left - - //while((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2 < 3) - - - + bt.printf("START\r\n"); //Go to tools tools_section(location, current); - bt.printf("Location 0 = %f", location[0]); - - /* - //////////////////////////////// without predefined wavegaps////////////////////////////////////////////// - current=0; - if(location[0]< 75){ - turnRight(); - current=wall_follow(LEFT,FORWARD); - if(current == 0)turnLeft(); - else{ - direction[0]= RIGHT; - turnLeft(); - overBump(); - } - } - else if(location[0]>=75 || current == 0){ - turnLeft(); - wall_follow2(RIGHT,FORWARD); - } - - - - - - - // left or right - - - location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - - - - - - - - leftTurn(); - //wall_follow2(RIGHT); - rightTurn(); - - - - bt.printf("LOCATION %f\n\r",location); - - motors.stopBothMotors(); -// leftTurn(); -// wait(1); -// rightTurn(); -*/ - + mid_section(location, current, direction); + mid_section2(location, current, direction); + + + } - + void errFunction(void) { //Nothing } - -void us_distance(void) + +void us_distance(int side) +{ + if(side == LEFT) { + rangeFinderLeft.startMeas(); + wait_us(20); + rangeFinderLeft.getMeas(range); + } else { + + rangeFinderRight.startMeas(); + wait_us(20); + rangeFinderRight.getMeas(range); + } + +} + +void alignParallel(int side, int turn, float limit) { - - rangeFinderLeft.startMeas(); - wait_us(20); - if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) { - pc.printf("Range = %f\n\r", range); + leftEncoder.reset(); + rightEncoder.reset(); + + if(turn == LEFT) { + motors.setMotor0Speed(MAX_SPEED); //right + motors.setMotor1Speed(-MAX_SPEED); //left + while(rightEncoder.getPulses() < 100); + + motors.stopBothMotors(); + + motors.setMotor0Speed(0.7*MAX_SPEED); //right + motors.setMotor1Speed(-0.7*MAX_SPEED); //left + } else { + motors.setMotor0Speed(-MAX_SPEED); //right + motors.setMotor1Speed(MAX_SPEED); //left + while(leftEncoder.getPulses() < 100); + + motors.stopBothMotors(); + + motors.setMotor0Speed(-0.7*MAX_SPEED); //right + motors.setMotor1Speed(0.7*MAX_SPEED); //left } + + usValue = 0; + + if(side == LEFT) { + while(1) { + us_distance(LEFT); + //bt.printf("Range %f \t OldValue %f\n\r",range, usValue); + if(range > usValue && usValue != 0 && range < limit) { + break; + } else { + usValue = range; + } + } + } else { + while(1) { + us_distance(RIGHT); + //bt.printf("Range %f \t OldValue %f\n\r",range, usValue); + if(range > usValue && usValue != 0 && range < limit) { + break; + } else { + usValue = range; + } + } + } + motors.stopBothMotors(); } - + float wall_follow(int side, int direction, int section) { - float location, wavegap; - int dir=1, set=5; - + float location, wavegap=0, set=4; + 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< 75) { + + while(location< 78) { 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); - if(side){ //Side = 1 for Left - rangeFinderLeft.startMeas(); - wait_ms(20); - rangeFinderLeft.getMeas(range); + if(side) { + us_distance(LEFT); + } else { + us_distance(RIGHT); } - else{ //Use right Ultrasonic - rangeFinderRight.startMeas(); - wait_ms(20); - rangeFinderRight.getMeas(range); - pc.printf("%d\r\n",range); - } - + if(range > 20) { wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - bt.printf("wavegap %f\r\n",wavegap); + //bt.printf("wavegap %f\r\n",wavegap); // AT WAVE OPENING!!!! - motors.setMotor1Speed(dir*MAX_SPEED);//left - motors.setMotor0Speed(dir*MAX_SPEED);//right - + motors.setMotor1Speed(dir*0.3*127);//left + motors.setMotor0Speed(dir*0.3*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 + + pid1.setProcessValue(range); + pid_return = pid1.compute(); + + 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.setMotor0Speed(dir*MAX_SPEED);//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.setMotor0Speed(dir*MAX_SPEED);//right - motors.setMotor1Speed(dir*MAX_SPEED);//left } - } - } + } return wavegap; } - + /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ - -void wall_follow2(int side, int direction, int section) + +void wall_follow2(int side, int direction, int section, float location) { - int SeeWaveGap = false, count=0, set=5; - float dir=1.0; - + int SeeWaveGap = false, dir=1; + float set=4, loc=0; + pid1.reset(); - + if(direction == BACKWARD) dir=-1; if(section == TOOLS)set= 5; - + leftEncoder.reset(); rightEncoder.reset(); - - while(1) { - + + while(loc + location < 80) { + loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + pid1.setInputLimits(0.0, set); pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); pid1.setSetPoint(set); - - if(side){ - rangeFinderLeft.startMeas(); - wait_ms(20); - rangeFinderLeft.getMeas(range); + + if(side) { + us_distance(LEFT); + } else { + us_distance(RIGHT); } - else{ - rangeFinderRight.startMeas(); - wait_ms(20); - rangeFinderRight.getMeas(range); - } - - + + /*************CHECK FOR WAVE OPENING*****************/ /* If after 20 ms the ultrasonic still sees 20+ cm */ /* then robot is at wave opening */ - + pc.printf("range %f\r\n",range); if(range > 20) { motors.stopBothMotors(); @@ -349,26 +254,24 @@ // AT WAVE OPENING!!!! break; } - + pid1.setProcessValue(range); pid_return = pid1.compute(); //bt.printf("Range: %f\n PID: %f\r\n", range, pid_return); - - if(pid_return > 0) { - if(side){ + + if(pid_return > 0) { + if(side) { motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right motors.setMotor1Speed(dir*MAX_SPEED);//left - } - else{ + } else { motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left motors.setMotor0Speed(dir*MAX_SPEED);//right } - }else if(pid_return < 0) { - if(side){ + } else if(pid_return < 0) { + if(side) { motors.setMotor0Speed(dir*MAX_SPEED);//right motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left - } - else{ + } else { motors.setMotor1Speed(dir*MAX_SPEED);//left motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right } @@ -377,214 +280,210 @@ motors.setMotor1Speed(dir*MAX_SPEED); } } + motors.stopBothMotors(); } - - -/* MODIFIED WALL_FOLLOW FOR NAVIGATION WITH WAVE OPENINGS PASSED IN */ -/* MEANT FOR RETURNING FROM OIL RIGS */ - -void wall_follow3(int ¤tLocation, int &WaveOpening) + + +void alignWithWall(int section) { - while(1) { - - - pid1.setInputLimits(0, 5); - pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); - pid1.setSetPoint(5.0); - - rangeFinderLeft.startMeas(); - wait_ms(100); - if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) { - //bt.printf("Range = %f\n", range); - } - - /*************CHECK FOR WAVE OPENING*****************/ - /* If after 100 ms the ultrasonic still sees 20+ cm */ - /* then robot is at wave opening */ - - - if(range > 20 ) { - currentLocation--; - } - - if( currentLocation == WaveOpening) { - // AT WAVE OPENING!!!! - - break; - } - - - pid1.setProcessValue(range); - pid_return = pid1.compute(); - bt.printf("Range: %f\n PID: %f", range, pid_return); - - if(pid_return > 0) { - motors.setMotor0Speed(MAX_SPEED - pid_return); - motors.setMotor1Speed(MAX_SPEED); - } else if(pid_return < 0) { - motors.setMotor0Speed(MAX_SPEED); - motors.setMotor1Speed(MAX_SPEED + pid_return); - } else { - motors.setMotor0Speed(MAX_SPEED); - motors.setMotor1Speed(MAX_SPEED); - } + usValue = 0; + + if(section == TOOLS) { + // turn at an angle + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(-MAX_SPEED); //right + motors.setMotor1Speed(0.4*MAX_SPEED); //left + while(rightEncoder.getPulses()>-1000); + motors.stopBothMotors(); + + //go backwards toward wall + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(-MAX_SPEED); //right + motors.setMotor1Speed(-MAX_SPEED); //left + while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300); + + motors.stopBothMotors(); + + // turn left towards wall + alignParallel(LEFT,LEFT,100); + } else { + // turn right towards wall + alignParallel(LEFT,RIGHT, 60); } } - + void rightTurn(void) { - float speedL, speedR; - - speedL=speedR= 0.4; - leftEncoder.reset(); rightEncoder.reset(); - motors.setMotor0Speed(-speedR*127);//right - motors.setMotor1Speed(speedL*127);//left - while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900); + motors.setMotor0Speed(-0.4*127);//right + motors.setMotor1Speed(0.4*127);//left + while(leftEncoder.getPulses()<1000 || rightEncoder.getPulses()>-1000); motors.stopBothMotors(); } - + void leftTurn(void) { leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(0.4*127);// right motors.setMotor1Speed(-0.4*127);// left - while(leftEncoder.getPulses()>-940 || rightEncoder.getPulses()<940); - motors.stopBothMotors(); -} - -void overBump(void){ - - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(MAX_SPEED); //right - motors.setMotor1Speed(MAX_SPEED); //left - while(abs(leftEncoder.getPulses()/PPR) + abs(rightEncoder.getPulses()/PPR)/2 < 1); - /* - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(MAX_SPEED); //right - motors.setMotor1Speed(MAX_SPEED); //left - while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <3); - */ + while(abs(leftEncoder.getPulses())<1075 || rightEncoder.getPulses()<1075); motors.stopBothMotors(); } -void alignWithWall(void){ - - leftEncoder.reset(); - rightEncoder.reset(); - - motors.setMotor0Speed(-MAX_SPEED); //right - motors.setMotor1Speed(0.4*MAX_SPEED); //left - while(rightEncoder.getPulses()>-1000); - motors.stopBothMotors(); - + + +void overBump(int wave) +{ + int preLeft=5000, preRight=5000 ; + leftEncoder.reset(); rightEncoder.reset(); - - motors.setMotor0Speed(-MAX_SPEED); //right - motors.setMotor1Speed(-MAX_SPEED); //left - while(abs(leftEncoder.getPulses()*11.12/PPR) < 3); - - motors.setMotor0Speed(0.1*MAX_SPEED); //right - motors.setMotor1Speed(0.1*MAX_SPEED); //left - - wait_ms(200); - - motors.stopBothMotors(); - + motors.setMotor0Speed(0.15*127); //right + motors.setMotor1Speed(0.15*127); //left + while((abs(leftEncoder.getPulses()) < 700 || abs(rightEncoder.getPulses())< 700) && preLeft!=0) { + preLeft=leftEncoder.getPulses(); + preRight=rightEncoder.getPulses(); + wait_ms(20); + if(leftEncoder.getPulses() == preLeft && rightEncoder.getPulses()== preRight) preLeft=preRight=0; + } + + if(wave == FAR) { + while(leftEncoder.getPulses() != preLeft && rightEncoder.getPulses()!= preRight) { + preLeft=leftEncoder.getPulses(); + preRight=rightEncoder.getPulses(); + wait_ms(20); + } + + motors.stopBothMotors(); + } + leftEncoder.reset(); rightEncoder.reset(); - - motors.setMotor0Speed(MAX_SPEED); //right - motors.setMotor1Speed(-MAX_SPEED); //left - while(rightEncoder.getPulses() < 260); + motors.setMotor0Speed(0.4*127); //right + motors.setMotor1Speed(0.4*127); //left + while(abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200); + + motors.stopBothMotors(); +} + +void tools_section(float* location, float ¤t) +{ + + wall_follow(LEFT,FORWARD, TOOLS); + // current position in reference to the starting position + current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + bt.printf("current %f \r\n",current); + + motors.stopBothMotors(); + + //Tool aquiring + wait(2); + // After tool is aquired + + alignWithWall(TOOLS); + current-=8; + wait_ms(100); + + us_distance(LEFT); - motors.setMotor0Speed(0.7*MAX_SPEED); //right - motors.setMotor1Speed(-0.7*MAX_SPEED); //left - float usValue = 0; - while(1){ - rangeFinderLeft.startMeas(); - wait_ms(100); - rangeFinderLeft.getMeas(range); - bt.printf("Range %f \t OldValue %f\n\r",range, usValue); - if(range > usValue && usValue != 0 && range < 25){ - break; - } else { - usValue = range; - } + if(range < 20) { + wall_follow2(LEFT,BACKWARD,TOOLS, current); + location[0]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + current= location[0]; + + /* // go backwards + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(-0.3*127); //right + motors.setMotor1Speed(-0.3*127); //left + while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150); + + motors.stopBothMotors(); + */ + + leftTurn(); + overBump(FAR); + } else { + location[0]= 77; + leftTurn(); + wait_ms(20); + overBump(FIRST_WAVE); } - - - - + + bt.printf("wavegap = %f\r\n",location[0]); } - -void tools_section(float *location, float ¤t){ - - //location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field - //Too Far from wall to see gap :( - wall_follow(LEFT,FORWARD,TOOLS); - - current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - - bt.printf("wavegap %f \t current %f \r\n",location[0],current); - - motors.stopBothMotors(); - ////////////////////////////////////////// determine shape and pick up tool /////////////////////////////////////////////////////// - alignWithWall(); - wait_ms(200); - - rangeFinderLeft.startMeas(); - wait_ms(100); - rangeFinderLeft.getMeas(range); - if(range < 10){ - - wall_follow2(LEFT,BACKWARD,TOOLS); - - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-0.9*MAX_SPEED); //right - motors.setMotor1Speed(-0.9*MAX_SPEED); //left - while(abs(leftEncoder.getPulses()*11.12/PPR) < 2); - motors.stopBothMotors(); - - // forward - - /* leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-MAX_SPEED); //right - motors.setMotor1Speed(-MAX_SPEED); //left - while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300); - motors.stopBothMotors(); - wait_ms(500);*/ - current = (abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - // REMOVED += because current should just be current pulses? - /* - wall_follow2(LEFT,FORWARD); - // backward - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(MAX_SPEED); //right - motors.setMotor1Speed(MAX_SPEED); //left - while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300); - motors.stopBothMotors(); - current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;*/ - - +void mid_section(float* location, float ¤t, int* direction) +{ + + motors.begin(); + alignWithWall(MID); + /* + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(MAX_SPEED); //right + motors.setMotor1Speed(MAX_SPEED); //left + while(abs(leftEncoder.getPulses())<75 || abs(rightEncoder.getPulses())<75); + motors.stopBothMotors(); + */ + bt.printf("mid section current = %f\r\n",current); + wall_follow2(LEFT,FORWARD,MID, current); + current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + bt.printf("after wf2 current = %f\r\n",current); + + if(current != 0) { + direction[0]= RIGHT; + current+= location[0]; + location[1]= current; + } else { + current=location[0]; + direction[0]= LEFT; + wall_follow2(LEFT,BACKWARD,MID,current); + location[1]= location[0]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + } + + bt.printf("wavegap2 = %f\r\n",location[1]); leftTurn(); - - //Go over - overBump(); - } else { - + overBump(FAR); + // go forward + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(0.2*127); //right + motors.setMotor1Speed(0.2*127); //left + while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300); + motors.stopBothMotors(); + +} + +void mid_section2(float* location, float ¤t, int* direction) +{ + + motors.begin(); + rightTurn(); + alignWithWall(MID); + wall_follow2(LEFT,FORWARD,MID, current); + current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + + if(current != 0) { + direction[1]= RIGHT; + current+= location[1]; + location[2]= current; + } else { + current=location[1]; + direction[1]= LEFT; + wall_follow2(LEFT,BACKWARD,MID,current); + location[2]= location[1]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + } + leftTurn(); - //Go over - overBump(); - } - + overBump(FAR); } - \ No newline at end of file + +void rig_section(float* location, float ¤t, int* direction, int rig) +{ + + +} \ No newline at end of file