Yeah
Dependencies: HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of NavigationTest by
Diff: main.cpp
- Revision:
- 6:f5c26372b2d0
- Parent:
- 5:70ccef3734ae
- Child:
- 7:78745a518957
--- a/main.cpp Thu Mar 20 17:47:31 2014 +0000 +++ b/main.cpp Fri Mar 21 21:26:50 2014 +0000 @@ -23,7 +23,9 @@ #define BACKWARD (0) #define TOOLS (0) #define MID (1) -#define RIGS (2) +#define FIRST_WAVE (0) +#define FAR (1) + float range, pid_return; void errFunction(void); @@ -44,14 +46,17 @@ //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 align(float speed); +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); //Variables @@ -64,75 +69,11 @@ bt.baud(115200); motors.begin(); - - //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) - - - //Go to tools tools_section(location, current); - - //////////////////////////////// without predefined wavegaps////////////////////////////////////////////// - /* current=0; - if(location[0]< 75){ - rightTurn(); - current=wall_follow(LEFT,FORWARD,MID); - if(current != 0) direction[0]= RIGHT; - else{ - direction[0]= LEFT; - wall_follow2(LEFT,BACKWARD,MID); - location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - - 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(); - } - leftTurn(); - } - else{ - leftTurn(); - direction[0]= LEFT; - wall_follow2(RIGHT,FORWARD,MID); - location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - rightTurn(); - } - - overBump(); - - */ - - - // left or right - - - - - - - - - - //leftTurn(); - //wall_follow2(RIGHT); - // rightTurn(); - + mid_section(location, current, direction); + mid_section2(location, current, direction); - - bt.printf("LOCATION %f\n\r",location); - - //overBump(); - //motors.stopBothMotors(); - //leftTurn(); - //wait(1); - //rightTurn(); } @@ -154,7 +95,7 @@ float wall_follow(int side, int direction, int section) { - float location, wavegap=0, set=5; + float location, wavegap=0, set=4; int dir=1; pid1.reset(); @@ -167,7 +108,7 @@ 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); @@ -189,32 +130,36 @@ wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; bt.printf("wavegap %f\r\n",wavegap); // AT WAVE OPENING!!!! + motors.setMotor1Speed(dir*0.3*127);//left + motors.setMotor0Speed(dir*0.3*127);//right } + else{ - pid1.setProcessValue(range); - pid_return = pid1.compute(); + pid1.setProcessValue(range); + pid_return = pid1.compute(); - if(pid_return > 0) { - if(side){ - motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right + 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; @@ -222,21 +167,22 @@ /* 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, dir=1; - float set=5; + float set=4, loc; pid1.reset(); if(direction == BACKWARD) dir=-1; - if(section == TOOLS)set= 6.5; + if(section == TOOLS)set= 5; leftEncoder.reset(); rightEncoder.reset(); - while(1) { - + while(loc - location ) { + location=(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); @@ -292,76 +238,68 @@ 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) -{ - while(1) { - - - pid1.setInputLimits(0, 5); - pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); - pid1.setSetPoint(5.0); - +void alignWithWall(int section){ + float 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()) < 350 || abs(rightEncoder.getPulses()) < 350); + + motors.stopBothMotors(); + + // turn left towards wall + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(MAX_SPEED); //right + motors.setMotor1Speed(-MAX_SPEED); //left + while(rightEncoder.getPulses() < 100); + + motors.stopBothMotors(); + } + else{ + // turn right towards wall + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(-MAX_SPEED); //right + motors.setMotor1Speed(MAX_SPEED); //left + while(abs(rightEncoder.getPulses()) < 300); + + motors.stopBothMotors(); + } + + // align using the distance of the wall + motors.setMotor0Speed(0.7*MAX_SPEED); //right + motors.setMotor1Speed(-0.7*MAX_SPEED); //left + + usValue = 0; + while(1){ 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!!!! - + wait_ms(20); + rangeFinderLeft.getMeas(range); + bt.printf("Range %f \t OldValue %f\n\r",range, usValue); + if(range > usValue && usValue != 0 && range < 25){ 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 = range; } } -} - -void align(float speed){ - float left, right; - left=right=speed; - - if(abs(leftEncoder.getPulses())> rightEncoder.getPulses()){ - if(speed>0)left-=.01; - else left+=.01; - } - else{ - if(speed>0) right+=.01; - else right-=.01; - } - motors.setMotor0Speed(right*127);//right - motors.setMotor1Speed(left*127);//left - pc.printf("speed left %f right %f\r\n\n",left, right); - + motors.stopBothMotors(); } void rightTurn(void) @@ -370,7 +308,7 @@ rightEncoder.reset(); motors.setMotor0Speed(-0.4*127);//right motors.setMotor1Speed(0.4*127);//left - while(leftEncoder.getPulses()<1030 || rightEncoder.getPulses()>-1030); + while(leftEncoder.getPulses()<1000 || rightEncoder.getPulses()>-1000); motors.stopBothMotors(); } @@ -380,105 +318,161 @@ rightEncoder.reset(); motors.setMotor0Speed(0.4*127);// right motors.setMotor1Speed(-0.4*127);// left - while(abs(leftEncoder.getPulses())<1120 || rightEncoder.getPulses()<1120); //align(0.4); + while(abs(leftEncoder.getPulses())<1075 || rightEncoder.getPulses()<1075); motors.stopBothMotors(); } -void slightRight(void) -{ - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-0.4*127);//right - motors.setMotor1Speed(0.4*127);//left - while(leftEncoder.getPulses()<515 || rightEncoder.getPulses()>-515); - motors.stopBothMotors(); -} -void slightLeft(void) -{ - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(0.4*127);// right - motors.setMotor1Speed(-0.4*127);// left - while(abs(leftEncoder.getPulses())<400 || rightEncoder.getPulses()< 400); //align(0.4); - motors.stopBothMotors(); -} -void overBump(void){ +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) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <9)//align(MAX_SPEED); + 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(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <6); - + 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){ - location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field + 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("wavegap %f \t current %f \r\n",location[0],current); + bt.printf("current %f \r\n",current); motors.stopBothMotors(); - slightRight(); - //backward + //Tool aquiring + wait(2); + // After tool is aquired + + alignWithWall(TOOLS); + current-=8; + wait_ms(100); + + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(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 mid_section(float* location, float ¤t, int* direction){ + + motors.begin(); + rightTurn(); + alignWithWall(MID); + /* leftEncoder.reset(); rightEncoder.reset(); - motors.setMotor0Speed(-MAX_SPEED);//right - motors.setMotor1Speed(-MAX_SPEED);//left - while(abs(leftEncoder.getPulses())<500 || abs(rightEncoder.getPulses())<500); + 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(); + 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(); - slightLeft(); +} + +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); - /*leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(MAX_SPEED);//right - motors.setMotor1Speed(MAX_SPEED);//left - while(abs(leftEncoder.getPulses())<100 || abs(rightEncoder.getPulses())<100);*/ + 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(); + overBump(FAR); +} + +void rig_section(float* location, float ¤t, int* direction, int rig){ - ////////////////////////////////////////// determine shape and pick up tool /////////////////////////////////////////////////////// - - wall_follow2(LEFT,BACKWARD,TOOLS); - /* - if(current >location[0]){ - wall_follow2(LEFT,BACKWARD,TOOLS); - wait_ms(1000); - // back - 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(); - current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - } - else{ - wall_follow2(LEFT,FORWARD,TOOLS); - // backward - wait_ms(1000); - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-MAX_SPEED); //right - motors.setMotor1Speed(-MAX_SPEED); //left - while(abs(leftEncoder.getPulses())<150 || abs(rightEncoder.getPulses())<150); - motors.stopBothMotors(); - current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - }*/ - wait_ms(1000); - leftTurn(); - - //Go over - overBump(); - -} \ No newline at end of file +} \ No newline at end of file