Yeah
Dependencies: HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of NavigationTest by
Diff: main.cpp
- Revision:
- 5:70ccef3734ae
- Parent:
- 4:f2333e66ec2c
- Child:
- 6:f5c26372b2d0
diff -r f2333e66ec2c -r 70ccef3734ae main.cpp --- a/main.cpp Tue Mar 18 23:11:28 2014 +0000 +++ b/main.cpp Thu Mar 20 17:47:31 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) @@ -24,11 +24,11 @@ #define TOOLS (0) #define MID (1) #define RIGS (2) - + float range, pid_return; void errFunction(void); bool cRc; - + //Hardware Initialization Serial bt(p13,p14); Serial pc(USBTX,USBRX); @@ -39,10 +39,10 @@ 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_follow3(int ¤tLocation, int &WaveOpening); @@ -51,14 +51,15 @@ void us_distance(void); void tools_section(float* location, float ¤t); void overBump(void); - +void align(float speed); + //Variables - + int main(void) { float location[3], current=0; int direction[3]; - + pc.baud(115200); bt.baud(115200); motors.begin(); @@ -70,38 +71,48 @@ //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; + /* current=0; if(location[0]< 75){ - turnRight(); - current=wall_follow(LEFT,FORWARD); - if(current == 0)turnLeft(); + rightTurn(); + current=wall_follow(LEFT,FORWARD,MID); + if(current != 0) direction[0]= RIGHT; else{ - direction[0]= RIGHT; - turnLeft(); - overBump(); + 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 if(location[0]>=75 || current == 0){ - turnLeft(); - wall_follow2(RIGHT,FORWARD); + 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 - location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; @@ -109,27 +120,28 @@ - leftTurn(); + //leftTurn(); //wall_follow2(RIGHT); - rightTurn(); + // rightTurn(); - - + + bt.printf("LOCATION %f\n\r",location); - - motors.stopBothMotors(); -// leftTurn(); -// wait(1); -// rightTurn(); -*/ - + + //overBump(); + //motors.stopBothMotors(); + //leftTurn(); + //wait(1); + //rightTurn(); + + } - + void errFunction(void) { //Nothing } - + void us_distance(void) { pc.printf("Ultra Sonic\n\r"); @@ -139,16 +151,16 @@ pc.printf("Range = %f\n\r", range); } } - + float wall_follow(int side, int direction, int section) { - float location, wavegap; - int dir=1, set=5; + float location, wavegap=0, set=5; + int dir=1; pid1.reset(); if(direction == BACKWARD) dir=-1; - if(section == TOOLS)set= 9; + if(section == TOOLS)set= 10; leftEncoder.reset(); rightEncoder.reset(); @@ -181,7 +193,7 @@ pid1.setProcessValue(range); pid_return = pid1.compute(); - + if(pid_return > 0) { if(side){ motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right @@ -207,27 +219,28 @@ } return wavegap; } - + /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ - + void wall_follow2(int side, int direction, int section) { - int SeeWaveGap = false, count=0, dir=1, set=5; + int SeeWaveGap = false, dir=1; + float set=5; pid1.reset(); if(direction == BACKWARD) dir=-1; - if(section == TOOLS)set= 9; + if(section == TOOLS)set= 6.5; leftEncoder.reset(); rightEncoder.reset(); - + while(1) { - - pid1.setInputLimits(0.0, 5.0); + + pid1.setInputLimits(0.0, set); pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); - pid1.setSetPoint(5.0); - + pid1.setSetPoint(set); + if(side){ rangeFinderLeft.startMeas(); wait_ms(20); @@ -238,12 +251,12 @@ 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(); @@ -251,11 +264,11 @@ // 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){ motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right @@ -280,46 +293,46 @@ } } } - - + + /* 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); - + 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); @@ -332,28 +345,61 @@ } } } - + +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); + +} + 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()<1050 || rightEncoder.getPulses()>-1050); + motors.setMotor0Speed(-0.4*127);//right + motors.setMotor1Speed(0.4*127);//left + while(leftEncoder.getPulses()<1030 || rightEncoder.getPulses()>-1030); motors.stopBothMotors(); } - + void leftTurn(void) -{ +{ leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(0.4*127);// right motors.setMotor1Speed(-0.4*127);// left - while(leftEncoder.getPulses()>-1200 || rightEncoder.getPulses()<1200); + while(abs(leftEncoder.getPulses())<1120 || rightEncoder.getPulses()<1120); //align(0.4); + 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(); } @@ -361,56 +407,78 @@ leftEncoder.reset(); rightEncoder.reset(); - motors.setMotor0Speed(127*.15); //right - motors.setMotor1Speed(127*.15); //left - while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <9); + 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); 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()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <6); motors.stopBothMotors(); } - + void tools_section(float* location, float ¤t){ location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field 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(); + motors.stopBothMotors(); + + slightRight(); + //backward + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(-MAX_SPEED);//right + motors.setMotor1Speed(-MAX_SPEED);//left + while(abs(leftEncoder.getPulses())<500 || abs(rightEncoder.getPulses())<500); + motors.stopBothMotors(); + + slightLeft(); + + /*leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(MAX_SPEED);//right + motors.setMotor1Speed(MAX_SPEED);//left + while(abs(leftEncoder.getPulses())<100 || abs(rightEncoder.getPulses())<100);*/ + + + ////////////////////////////////////////// determine shape and pick up tool /////////////////////////////////////////////////////// + wall_follow2(LEFT,BACKWARD,TOOLS); + /* if(current >location[0]){ wall_follow2(LEFT,BACKWARD,TOOLS); - wait_ms(500); - // forward - /* leftEncoder.reset(); + wait_ms(1000); + // back + leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-MAX_SPEED); //right motors.setMotor1Speed(-MAX_SPEED); //left - while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300); + while(abs(leftEncoder.getPulses())<75 || abs(rightEncoder.getPulses())<75); motors.stopBothMotors(); - wait_ms(500);*/ current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; } - else{/* - wall_follow2(LEFT,FORWARD); + 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())<300 || abs(rightEncoder.getPulses())<300); + 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;*/ - } - + 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