revise
Dependencies: HCSR04 PID PololuQik2 QEI mbed-rtos Sharp
Diff: main.cpp
- Revision:
- 11:d67a3958127a
- Parent:
- 10:c57f6a5042d7
- Child:
- 12:168cb595f98e
diff -r c57f6a5042d7 -r d67a3958127a main.cpp --- a/main.cpp Thu Mar 27 22:40:22 2014 +0000 +++ b/main.cpp Sat Mar 29 20:21:55 2014 +0000 @@ -26,7 +26,6 @@ #define TOOLS (0) #define MID (1) #define RIGS (2) -#define FIRST_WAVE (0) #define FAR (1) @@ -55,6 +54,7 @@ void leftTurn(void); void slightleft(void); void rightTurn(void); +void slightMove(int direction, float pulses); void us_distance(void); void tools_section(float* location, float ¤t); void mid_section(float* location, float ¤t, int* direction); @@ -69,30 +69,33 @@ int main(void) { - float location[3], current=0; + float location[3], current=4; int direction[3]; double distance; pc.baud(115200); - bt.baud(115200); + pc.baud(115200); motors.begin(); - bt.printf("START\r\n"); + pc.printf("START\r\n"); //Go to tools tools_section(location, current); mid_section(location, current, direction); - //mid_section2(location, current, direction); - /* while(1) { - //bt.printf("IR %f\r\n", US.getDistance()); - rangeFinderLeft.startMeas(); - wait_ms(20); + mid_section2(location, current, direction); + /*while(1) { + pc.printf("IR %f\r\n", IR.getDistance()); + /*rangeFinderLeft.startMeas(); + rangeFinderRight.startMeas(); + wait_ms(38); rangeFinderLeft.getMeas(range); - bt.printf("Range = %f\n\r", 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); //leftTurn(); //rightTurn(); @@ -116,9 +119,9 @@ float wall_follow(int side, int direction, int section) { - float location, wavegap=0, set=5; + float location, wavegap=0, set=6; int dir=1; - + pid1.reset(); if(direction == BACKWARD) dir=-1; @@ -129,29 +132,30 @@ location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - while(location< 73) { + while(location< 69) { location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + pc.printf("LOCATION %f\r\n", location); pid1.setInputLimits(0, set); pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); pid1.setSetPoint(set); if(side) { rangeFinderLeft.startMeas(); - wait_ms(38); + wait_ms(20); rangeFinderLeft.getMeas(range); } else { rangeFinderRight.startMeas(); - wait_ms(38); + 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); + //pc.printf("wavegap %f\r\n",wavegap); // AT WAVE OPENING!!!! - motors.setMotor1Speed(dir*0.4*127);//left - motors.setMotor0Speed(dir*0.4*127);//right + motors.setMotor1Speed(dir*0.25*127);//left + motors.setMotor0Speed(dir*0.25*127);//right } else { pid1.setProcessValue(range); @@ -186,23 +190,31 @@ void wall_follow2(int side, int direction, int section, float location) { - bool SeeWaveGap = false, dir=1, limit=83; - float set=5, loc=0; + int dir=1, limit=90; + float set=6, loc=0; pid1.reset(); - if(direction == BACKWARD) dir=-1; + if(direction == BACKWARD){ + dir=-1; + limit = 100; + } if(section == TOOLS){ - set= 5; - limit = 78; + set= 6; + limit = 89; } - leftEncoder.reset(); rightEncoder.reset(); - - while(dir*loc + location <= limit) { + + 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)) { 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); @@ -210,17 +222,17 @@ if(side) { rangeFinderLeft.startMeas(); - wait_ms(38); + wait_ms(20); rangeFinderLeft.getMeas(range); - } else { + } else{ rangeFinderRight.startMeas(); - wait_ms(38); + wait_ms(20); rangeFinderRight.getMeas(range); } if(section == RIGS){ rangeFinderRight.startMeas(); - wait_ms(38); + wait_ms(20); rangeFinderRight.getMeas(range); if(range< 20){ motors.stopBothMotors(); @@ -229,23 +241,18 @@ } - //bt.printf("wall follow 2 range %f\r\n",range); - //bt.printf("loc+location = %f\r\n", loc+location); - if(range > 20 && !SeeWaveGap) { - motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right - motors.setMotor1Speed(dir*MAX_SPEED);//left - wait_ms(30); - SeeWaveGap = true; - } else if(range > 20 && SeeWaveGap){ + //pc.printf("wall follow 2 range %f\r\n",range); + //pc.printf("loc+location = %f\r\n", loc+location); + if(range > 20 ) { motors.stopBothMotors(); - bt.printf("wavegap\r\n"); + pc.printf("wavegap\r\n"); // AT WAVE OPENING!!!! break; } else { pid1.setProcessValue(range); pid_return = pid1.compute(); - //bt.printf("Range: %f\n PID: %f\r\n", range, pid_return); + //pc.printf("Range: %f\n PID: %f\r\n", range, pid_return); if(pid_return > 0) { if(side) { @@ -268,6 +275,12 @@ motors.setMotor1Speed(dir*MAX_SPEED); } }} + + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(dir*-0.25*127); //right + motors.setMotor1Speed(dir*-0.25*127); //left + while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50); motors.stopBothMotors(); } @@ -277,6 +290,7 @@ float usValue = 0; if(section == TOOLS) { + pc.printf("tools section align\r\n"); // turn at an angle leftEncoder.reset(); rightEncoder.reset(); @@ -288,10 +302,9 @@ //go backwards toward wall leftEncoder.reset(); rightEncoder.reset(); - motors.setMotor0Speed(-MAX_SPEED); //right - motors.setMotor1Speed(-MAX_SPEED); //left + motors.setMotor0Speed(-0.25*127); //right + motors.setMotor1Speed(-0.25*127); //left while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300); - motors.stopBothMotors(); // turn left towards wall @@ -299,24 +312,31 @@ rightEncoder.reset(); motors.setMotor0Speed(MAX_SPEED); //right motors.setMotor1Speed(-MAX_SPEED); //left - while(rightEncoder.getPulses() < 10 || abs(leftEncoder.getPulses()) < 10); + while(rightEncoder.getPulses() < 100 || abs(leftEncoder.getPulses()) < 100); motors.stopBothMotors(); - - motors.setMotor0Speed(0.7*MAX_SPEED); //right - motors.setMotor1Speed(-0.7*MAX_SPEED); //left - } else { + + // turning left + motors.setMotor0Speed(0.9*MAX_SPEED); //right + motors.setMotor1Speed(-0.9*MAX_SPEED); //left + + }else { + //rightTurn(); + pc.printf("in mid section align\r\n"); + // turn right towards wall rightTurn(); - motors.setMotor0Speed(-0.7*MAX_SPEED); //right - motors.setMotor1Speed(0.7*MAX_SPEED); //left + + // turning left towards wall + motors.setMotor0Speed(0.9*MAX_SPEED); //right + motors.setMotor1Speed(-0.9*MAX_SPEED); //left } - + usValue = 0; while(1) { rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); - //bt.printf("Range %f \t OldValue %f\n\r",range, usValue); + pc.printf("Range %f \t OldValue %f\n\r",range, usValue); if(range > usValue && usValue != 0 && range < 25) { break; } else { @@ -333,20 +353,12 @@ rightEncoder.reset(); motors.setMotor0Speed(-0.5*127);//right motors.setMotor1Speed(0.5*127);//left - while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900); + while(abs(leftEncoder.getPulses())<1100 || abs(rightEncoder.getPulses())<1100); motors.stopBothMotors(); } void leftTurn(void) { - /* - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(0.4*MAX_SPEED); //right - motors.setMotor1Speed(-MAX_SPEED); //left - while(abs(leftEncoder.getPulses())<2500); - motors.stopBothMotors(); - */ motors.begin(); leftEncoder.reset(); rightEncoder.reset(); @@ -362,7 +374,25 @@ rightEncoder.reset(); motors.setMotor0Speed(0.5*127);// right motors.setMotor1Speed(-0.5*127);// left - while(abs(leftEncoder.getPulses())<100 || rightEncoder.getPulses()<100); + while(abs(leftEncoder.getPulses())<90 || 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); + + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(dir*-0.25*127); //right + motors.setMotor1Speed(dir*-0.25*127); //left + while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50); motors.stopBothMotors(); } @@ -394,44 +424,33 @@ int preLeft=5000, preRight=5000, out=0; motors.begin(); - + // slight backwards leftEncoder.reset(); rightEncoder.reset(); - motors.setMotor0Speed(-0.2*127); //right - motors.setMotor1Speed(-0.2*127); //left - while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses())< 50); + 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.2*127); //right - motors.setMotor1Speed(0.2*127); //left - while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){ + motors.setMotor0Speed(0.3*127); //right + motors.setMotor1Speed(0.3*127); //left + while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) && preLeft!=0 && IR.getDistance() >20 ){ preLeft=leftEncoder.getPulses(); preRight=rightEncoder.getPulses(); - wait_ms(100); - //bt.printf(" first while left %d right %d \r\n", preLeft, preRight); + 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(); - wait(2); - /* - motors.stopBothMotors(); - motors.setMotor0Speed(0.15*127); //right - motors.setMotor1Speed(0.15*127); //left - preLeft=preRight=5000 ; - leftEncoder.reset(); - rightEncoder.reset(); - */ -// while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){ - /* preLeft=leftEncoder.getPulses(); - preRight=rightEncoder.getPulses(); - bt.printf("second while left %d right %d \r\n", preLeft, preRight); - wait_ms(200); - if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0; - }*/ leftEncoder.reset(); rightEncoder.reset(); @@ -452,12 +471,14 @@ 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(); - wait(2); motors.begin(); preLeft=preRight=5000 ; @@ -465,19 +486,41 @@ 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(IR.getDistance() > 38) break; + + preLeft=leftEncoder.getPulses(); + preRight=rightEncoder.getPulses(); + wait_ms(200); + } + } + else if( section == MID ){ + while(IR.getDistance() > 20 && (abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200) && (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()) < 200 || abs(rightEncoder.getPulses()) < 200); - if(section == TOOLS || section == MID) { - while(IR.getDistance() > 20 ) { - //bt.printf("IR %f\r\n", IR.getDistance()); - //bt.printf("third while left %d right %d \r\n", preLeft, preRight); - } - } else while((abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses())< 200)); - + motors.stopBothMotors(); + + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(-.25*127); //right motors.setMotor1Speed(-.25*127); //left - wait_ms(10); + while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10)); + motors.stopBothMotors(); - wait(2); + wait_ms(50); motors.begin(); } @@ -485,36 +528,24 @@ 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.setMotor0Speed(-.2*127); //right - motors.setMotor1Speed(-.2*127); //left - wait_ms(5); + motors.setMotor0Speed(-.3*127); //right + motors.setMotor1Speed(-.3*127); //left + wait_ms(10); motors.stopBothMotors(); - - motors.setMotor0Speed(.2*127); //right - motors.setMotor1Speed(.2*127); //left - while(IR.getDistance()>6); - - motors.setMotor0Speed(-.2*127); //right - motors.setMotor1Speed(-.2*127); //left - 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= 78; + //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); @@ -522,105 +553,124 @@ if(range < 20) { wall_follow2(LEFT,BACKWARD,TOOLS, current); - + 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]; - + pc.printf("current %f \r\n",current); + // go backwards leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-MAX_SPEED); //right motors.setMotor1Speed(-MAX_SPEED); //left while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120); - + // hard stop leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(MAX_SPEED); //right motors.setMotor1Speed(MAX_SPEED); //left - while(abs(leftEncoder.getPulses()) < 40 || abs(rightEncoder.getPulses())< 40); + while(abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses())< 10); motors.stopBothMotors(); - wait_ms(500); + wait_ms(100); leftTurn(); slightleft(); overBump(TOOLS); } else { - location[0]= 77; + pc.printf("else greater than 20\r\n"); + location[0]= current; leftTurn(); - wait_ms(20); - overBump(FIRST_WAVE); + overBump(TOOLS); } - bt.printf("First Wavegap = %f\r\n",location[0]); + pc.printf("First Wavegap = %f\r\n",location[0]); } void mid_section(float* location, float ¤t, int* direction) { - motors.begin(); - if(IR.getDistance() > 20){ + if(IR.getDistance() > 38){ direction[0]= STRAIGHT; + overBump(MID); return; } - + pc.printf("before align with wall \r\n"); alignWithWall(MID); + wait_ms(100); - bt.printf("mid section current = %f\r\n",current); + pc.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) { + 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; - 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); + location[1]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + if(location[1] < 18){ + slightMove(FORWARD, 50); + } } - bt.printf("wavegap2 = %f\r\n",location[1]); + pc.printf("wavegap2 = %f\r\n",location[1]); leftTurn(); - overBump(TOOLS); + slightleft(); + + wait_ms(100); + + overBump(MID); // go forward - leftEncoder.reset(); + /* 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(); + motors.stopBothMotors();*/ } void mid_section2(float* location, float ¤t, int* direction) { - motors.begin(); - if(IR.getDistance() > 20){ + pc.printf("mid section 2\r\n"); + + if(IR.getDistance() > 38){ direction[0]= STRAIGHT; + overBump(RIGS); return; } alignWithWall(MID); + pc.printf("midsection 2 alignt with wall mid \r\n"); wall_follow2(LEFT,FORWARD,MID, current); - current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - - if(current != 0) { + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + pc.printf("midseection 2 after wf2"); + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range); + + if(range > 20 ) { 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); + location[2]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } leftTurn(); + slightleft(); overBump(RIGS); + pc.printf("overbump rigs\r\n"); } void rig_section(float* location, float ¤t, int* direction, int rig)