Yeah
Dependencies: HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of NavigationTest by
Diff: main.cpp
- Revision:
- 9:f34700716f1d
- Parent:
- 8:11ef93eebe07
- Child:
- 10:2aa70a504c18
diff -r 11ef93eebe07 -r f34700716f1d main.cpp --- a/main.cpp Sat Mar 22 21:47:14 2014 +0000 +++ b/main.cpp Thu Mar 27 17:49:30 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) @@ -21,18 +21,18 @@ #define LEFT (1) #define RIGHT (0) #define FORWARD (1) -#define BACKWARD (0) +#define BACKWARD (0) #define TOOLS (0) #define MID (1) -#define RIGS (2) +#define RIGS (2) #define FIRST_WAVE (0) #define FAR (1) - - -float range, pid_return; + + +float range, range2, pid_return; void errFunction(void); bool cRc; - + //Hardware Initialization Serial bt(p13,p14); Serial pc(USBTX,USBRX); @@ -44,14 +44,15 @@ QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING); Sharp IR(p20); //InterruptIn encoder(p29); - - + + //Functions - + float wall_follow(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 slightleft(void); void rightTurn(void); void us_distance(void); void tools_section(float* location, float ¤t); @@ -60,37 +61,46 @@ void rig_section(float* location, float ¤t, int* direction, int rig); void overBump(int section); void alignWithWall(int section); - + //Variables - + int main(void) { float location[3], current=0; int direction[3]; double distance; - + pc.baud(115200); bt.baud(115200); motors.begin(); - + bt.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", IR.getDistance()); + //mid_section2(location, current, direction); + /* while(1) { + //bt.printf("IR %f\r\n", US.getDistance()); + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range); + bt.printf("Range = %f\n\r", range); + wait_ms(200); - } - */ - + }*/ + + + //leftTurn(); + //rightTurn(); + + } - + void errFunction(void) { //Nothing } - + void us_distance(void) { pc.printf("Ultra Sonic\n\r"); @@ -100,71 +110,67 @@ pc.printf("Range = %f\n\r", range); } } - + float wall_follow(int side, int direction, int section) { float location, wavegap=0, set=5; 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< 78) { + + while(location< 73) { 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){ + if(side) { rangeFinderLeft.startMeas(); wait_ms(38); rangeFinderLeft.getMeas(range); - } - else{ + } else { rangeFinderRight.startMeas(); wait_ms(38); 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); // AT WAVE OPENING!!!! - motors.setMotor1Speed(dir*0.3*127);//left - motors.setMotor0Speed(dir*0.3*127);//right - } - else{ - + motors.setMotor1Speed(dir*0.4*127);//left + motors.setMotor0Speed(dir*0.4*127);//right + } else { + pid1.setProcessValue(range); pid_return = pid1.compute(); - + if(pid_return > 0) { - if(side){ + 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 } - }else { + } else { motors.setMotor0Speed(dir*MAX_SPEED);//right motors.setMotor1Speed(dir*MAX_SPEED);//left } @@ -172,45 +178,44 @@ } return wavegap; } - + /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ - + void wall_follow2(int side, int direction, int section, float location) { int SeeWaveGap = false, dir=1; float set=5, loc=0; - + pid1.reset(); - + if(direction == BACKWARD) dir=-1; if(section == TOOLS)set= 5; - + leftEncoder.reset(); rightEncoder.reset(); - - while(loc + dir*location < 80) { + + while(dir*loc + location < 78) { 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){ + + if(side) { rangeFinderLeft.startMeas(); wait_ms(38); rangeFinderLeft.getMeas(range); - } - else{ + } else { rangeFinderRight.startMeas(); wait_ms(38); rangeFinderRight.getMeas(range); } - - + + /*************CHECK FOR WAVE OPENING*****************/ /* If after 20 ms the ultrasonic still sees 20+ cm */ /* then robot is at wave opening */ - + bt.printf("wall follow 2 range %f\r\n",range); bt.printf("loc+location = %f\r\n", loc+location); if(range > 20) { @@ -219,26 +224,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 } @@ -249,38 +252,39 @@ } motors.stopBothMotors(); } - - -void alignWithWall(int section){ + + +void alignWithWall(int section) +{ float usValue = 0; - - if(section == TOOLS){ + + if(section == TOOLS) { // turn at an angle leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-MAX_SPEED); //right + rightEncoder.reset(); + motors.setMotor0Speed(-1.2*MAX_SPEED); //right motors.setMotor1Speed(0.4*MAX_SPEED); //left while(rightEncoder.getPulses()>-1000); motors.stopBothMotors(); - + //go backwards toward wall leftEncoder.reset(); - rightEncoder.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 + + // turn left towards wall leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(MAX_SPEED); //right motors.setMotor1Speed(-MAX_SPEED); //left - while(rightEncoder.getPulses() < 100); - - motors.stopBothMotors(); - + while(rightEncoder.getPulses() < 10 || abs(leftEncoder.getPulses()) < 10); + + motors.stopBothMotors(); + motors.setMotor0Speed(0.7*MAX_SPEED); //right motors.setMotor1Speed(-0.7*MAX_SPEED); //left } else { @@ -288,165 +292,238 @@ motors.setMotor0Speed(-0.7*MAX_SPEED); //right motors.setMotor1Speed(0.7*MAX_SPEED); //left } - + usValue = 0; - while(1){ + while(1) { rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); //bt.printf("Range %f \t OldValue %f\n\r",range, usValue); - if(range > usValue && usValue != 0 && range < 25){ + if(range > usValue && usValue != 0 && range < 25) { break; } else { - usValue = range; + usValue = range; } } motors.stopBothMotors(); } - + void rightTurn(void) { + motors.begin(); leftEncoder.reset(); rightEncoder.reset(); - motors.setMotor0Speed(-0.4*127);//right - motors.setMotor1Speed(0.4*127);//left - while(leftEncoder.getPulses()<1000 || rightEncoder.getPulses()>-1000); + motors.setMotor0Speed(-0.5*127);//right + motors.setMotor1Speed(0.5*127);//left + while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900); motors.stopBothMotors(); } - + void leftTurn(void) -{ +{ + /* leftEncoder.reset(); rightEncoder.reset(); - motors.setMotor0Speed(0.4*127);// right - motors.setMotor1Speed(-0.4*127);// left + 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(); + motors.setMotor0Speed(0.5*127);// right + motors.setMotor1Speed(-0.5*127);// left while(abs(leftEncoder.getPulses())<1000 || rightEncoder.getPulses()<1000); motors.stopBothMotors(); } - - -void overBump(int section){ - int preLeft=5000, preRight=5000 ; +void slightleft(void){ + + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(0.5*127);// right + motors.setMotor1Speed(-0.5*127);// left + while(abs(leftEncoder.getPulses())<50 || rightEncoder.getPulses()<50); + motors.stopBothMotors(); +} + + +void overBump(int section) +{ + int preLeft=5000, preRight=5000, out=0; + + motors.begin(); + + 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.stopBothMotors(); leftEncoder.reset(); rightEncoder.reset(); - motors.setMotor0Speed(0.15*127); //right - motors.setMotor1Speed(0.15*127); //left + 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){ - preLeft=leftEncoder.getPulses(); - preRight=rightEncoder.getPulses(); - wait_ms(200); - bt.printf(" first while left %d right %d \r\n", preLeft, preRight); - if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0; + preLeft=leftEncoder.getPulses(); + preRight=rightEncoder.getPulses(); + wait_ms(100); + bt.printf(" first while left %d right %d \r\n", preLeft, preRight); + if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0; } motors.stopBothMotors(); - motors.setMotor0Speed(0.15*127); //right - motors.setMotor1Speed(0.15*127); //left - preLeft=preRight=5000 ; + 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(); - - 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(!out) { 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; + + 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 + } + if(abs(leftEncoder.getPulses()) <1000 || abs(leftEncoder.getPulses())<1000) out=1; } - + motors.stopBothMotors(); + wait(2); + motors.begin(); preLeft=preRight=5000 ; leftEncoder.reset(); rightEncoder.reset(); - motors.setMotor0Speed(MAX_SPEED); //right - motors.setMotor1Speed(MAX_SPEED); //left - - if(section == TOOLS){ + motors.setMotor0Speed(.25*127); //right + motors.setMotor1Speed(.25*127); //left + + 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)); - preLeft=leftEncoder.getPulses(); - preRight=rightEncoder.getPulses(); - bt.printf("third while left %d right %d \r\n", preLeft, preRight); - wait_ms(20); - if(leftEncoder.getPulses() == preLeft && rightEncoder.getPulses()== preRight){ - motors.setMotor0Speed(0.4*127); //right - motors.setMotor1Speed(0.4*127); //left - } - } - } - else while((abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses())< 300)); - + motors.setMotor0Speed(-.25*127); //right + motors.setMotor1Speed(-.25*127); //left + wait_ms(10); motors.stopBothMotors(); wait(2); - + motors.begin(); + } - -void tools_section(float* location, float ¤t){ - - wall_follow(LEFT,FORWARD, TOOLS); + +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); + wall_follow2(LEFT,FORWARD,MID, current); + current= 78; + rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); - - if(range < 20){ + + if(range < 20) { wall_follow2(LEFT,BACKWARD,TOOLS, current); + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(-MAX_SPEED); //right + motors.setMotor1Speed(-MAX_SPEED); //left + while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120); + + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(MAX_SPEED); //right + motors.setMotor1Speed(MAX_SPEED); //left + while(abs(leftEncoder.getPulses()) < 40 || abs(rightEncoder.getPulses())< 40); + motors.stopBothMotors(); + + wait_ms(500); location[0]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); current= location[0]; - + leftTurn(); + slightleft(); overBump(TOOLS); - } - else{ - location[0]= 77; + } 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){ + +void mid_section(float* location, float ¤t, int* direction) +{ + + motors.begin(); - motors.begin(); + if(IR.getDistance() > 20) return; + alignWithWall(MID); 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){ + + if(current != 0) { direction[0]= RIGHT; current+= location[0]; location[1]= current; - } - else{ - current=location[0]; + } 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(); + leftTurn(); overBump(TOOLS); // go forward leftEncoder.reset(); @@ -455,33 +532,37 @@ 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(); -void mid_section2(float* location, float ¤t, int* direction){ + if(IR.getDistance() > 20) return; - motors.begin(); 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){ + + if(current != 0) { direction[1]= RIGHT; current+= location[1]; location[2]= current; - } - else{ - current=location[1]; + } 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(); + + leftTurn(); overBump(RIGS); -} - -void rig_section(float* location, float ¤t, int* direction, int rig){ - - +} + +void rig_section(float* location, float ¤t, int* direction, int rig) +{ + + } \ No newline at end of file