revise
Dependencies: HCSR04 PID PololuQik2 QEI mbed-rtos Sharp
Diff: main.cpp
- Revision:
- 10:c57f6a5042d7
- Parent:
- 9:f34700716f1d
- Child:
- 11:d67a3958127a
--- a/main.cpp Thu Mar 27 17:49:30 2014 +0000 +++ b/main.cpp Thu Mar 27 22:40:22 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) @@ -20,6 +20,7 @@ #define PPR (4331/4) #define LEFT (1) #define RIGHT (0) +#define STRAIGHT (2) #define FORWARD (1) #define BACKWARD (0) #define TOOLS (0) @@ -27,12 +28,12 @@ #define RIGS (2) #define FIRST_WAVE (0) #define FAR (1) - - + + float range, range2, pid_return; void errFunction(void); bool cRc; - + //Hardware Initialization Serial bt(p13,p14); Serial pc(USBTX,USBRX); @@ -44,10 +45,10 @@ 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); @@ -61,19 +62,21 @@ void rig_section(float* location, float ¤t, int* direction, int rig); void overBump(int section); void alignWithWall(int section); +void UntilWall(int dir); + //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); @@ -85,22 +88,22 @@ 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"); @@ -110,25 +113,25 @@ 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< 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); @@ -142,7 +145,7 @@ 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); @@ -150,10 +153,10 @@ 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) { motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right @@ -178,29 +181,33 @@ } return wavegap; } - + /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ - + void wall_follow2(int side, int direction, int section, float location) { - int SeeWaveGap = false, dir=1; + bool SeeWaveGap = false, dir=1, limit=83; float set=5, loc=0; - + pid1.reset(); - + if(direction == BACKWARD) dir=-1; - if(section == TOOLS)set= 5; + if(section == TOOLS){ + set= 5; + limit = 78; + } + leftEncoder.reset(); rightEncoder.reset(); - - while(dir*loc + location < 78) { + + while(dir*loc + location <= limit) { 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(38); @@ -210,25 +217,36 @@ 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) { - motors.stopBothMotors(); + + if(section == RIGS){ + rangeFinderRight.startMeas(); + wait_ms(38); + rangeFinderRight.getMeas(range); + if(range< 20){ + motors.stopBothMotors(); + break; + } + } + + + //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){ + motors.stopBothMotors(); bt.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); - + if(pid_return > 0) { if(side) { motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right @@ -249,15 +267,15 @@ motors.setMotor0Speed(dir*MAX_SPEED); motors.setMotor1Speed(dir*MAX_SPEED); } - } + }} motors.stopBothMotors(); } - - + + void alignWithWall(int section) { float usValue = 0; - + if(section == TOOLS) { // turn at an angle leftEncoder.reset(); @@ -266,25 +284,25 @@ 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 leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(MAX_SPEED); //right motors.setMotor1Speed(-MAX_SPEED); //left 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 { @@ -292,7 +310,7 @@ motors.setMotor0Speed(-0.7*MAX_SPEED); //right motors.setMotor1Speed(0.7*MAX_SPEED); //left } - + usValue = 0; while(1) { rangeFinderLeft.startMeas(); @@ -307,7 +325,7 @@ } motors.stopBothMotors(); } - + void rightTurn(void) { motors.begin(); @@ -318,7 +336,7 @@ while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900); motors.stopBothMotors(); } - + void leftTurn(void) { /* @@ -334,20 +352,43 @@ rightEncoder.reset(); motors.setMotor0Speed(0.5*127);// right motors.setMotor1Speed(-0.5*127);// left - while(abs(leftEncoder.getPulses())<1000 || rightEncoder.getPulses()<1000); + while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100); motors.stopBothMotors(); } + 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); + while(abs(leftEncoder.getPulses())<100 || rightEncoder.getPulses()<100); motors.stopBothMotors(); } - +void UntilWall(int dir){ + + if(dir == BACKWARD) dir=-1; + + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(dir*0.2*127); //right + motors.setMotor1Speed(dir*0.2*127); //left + + range = 30; + + while(range > 20){ + rangeFinderRight.startMeas(); + wait_ms(20); + rangeFinderRight.getMeas(range); + } + + motors.setMotor0Speed(dir*-0.2*127); //right + motors.setMotor1Speed(dir*-0.2*127); //left + wait_ms(5); + motors.stopBothMotors(); +} + void overBump(int section) { int preLeft=5000, preRight=5000, out=0; @@ -369,7 +410,7 @@ preLeft=leftEncoder.getPulses(); preRight=rightEncoder.getPulses(); wait_ms(100); - bt.printf(" first while left %d right %d \r\n", preLeft, preRight); + //bt.printf(" first while left %d right %d \r\n", preLeft, preRight); if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0; } @@ -396,25 +437,25 @@ rightEncoder.reset(); motors.setMotor0Speed(0.3*127); //right motors.setMotor1Speed(0.3*127); //left - + while(!out) { preLeft=leftEncoder.getPulses(); preRight=rightEncoder.getPulses(); - + 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; + if(abs(leftEncoder.getPulses()) >1000 || abs(leftEncoder.getPulses())>1000) out=1; } - + motors.stopBothMotors(); wait(2); motors.begin(); @@ -424,36 +465,50 @@ rightEncoder.reset(); 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); + //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.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); // 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.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(); + + //Tool aquiring wait(2); // After tool is aquired - + alignWithWall(TOOLS); wait_ms(100); @@ -464,9 +519,13 @@ 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]; + leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-MAX_SPEED); //right @@ -479,11 +538,8 @@ 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); @@ -493,24 +549,27 @@ wait_ms(20); overBump(FIRST_WAVE); } - - bt.printf("wavegap = %f\r\n",location[0]); + + bt.printf("First Wavegap = %f\r\n",location[0]); } - + void mid_section(float* location, float ¤t, int* direction) { - + motors.begin(); - if(IR.getDistance() > 20) return; + if(IR.getDistance() > 20){ + direction[0]= STRAIGHT; + 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) { direction[0]= RIGHT; current+= location[0]; @@ -521,7 +580,7 @@ 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(TOOLS); @@ -532,20 +591,23 @@ 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(); - if(IR.getDistance() > 20) return; + if(IR.getDistance() > 20){ + direction[0]= STRAIGHT; + return; + } 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]; @@ -556,13 +618,43 @@ 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(RIGS); } - + void rig_section(float* location, float ¤t, int* direction, int rig) { - - + float loc; + + if(rig == 1) loc= 16; + else if(rig == 2) loc= 37; + else loc = 58; + + rightTurn(); + + + if(current > loc){ + UntilWall(BACKWARD); + current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + wall_follow2(RIGHT, BACKWARD, RIGS, current); + current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + + if((current- loc)>10){ + wall_follow2(RIGHT, BACKWARD, RIGS, current); + current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + } + } + else{ + UntilWall(FORWARD); + wall_follow2(RIGHT, FORWARD, RIGS, current); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + + if((current- loc)<-10){ + wall_follow2(RIGHT, FORWARD, RIGS, current); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + } + } + + } \ No newline at end of file