Yeah
Dependencies: HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of NavigationTest by
Diff: main.cpp
- Revision:
- 8:11ef93eebe07
- Parent:
- 7:78745a518957
- Child:
- 9:f34700716f1d
diff -r 78745a518957 -r 11ef93eebe07 main.cpp --- a/main.cpp Fri Mar 21 22:19:51 2014 +0000 +++ b/main.cpp Sat Mar 22 21:47:14 2014 +0000 @@ -6,6 +6,7 @@ #include "HCSR04.h" #include "stdio.h" #include "LPC17xx.h" +#include "Sharp.h" #define PIN_TRIGGERL (p12) #define PIN_ECHOL (p11) @@ -23,9 +24,10 @@ #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); @@ -40,6 +42,7 @@ PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc); QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING); QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING); +Sharp IR(p20); //InterruptIn encoder(p29); @@ -55,7 +58,7 @@ 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 overBump(int section); void alignWithWall(int section); //Variables @@ -64,6 +67,7 @@ { float location[3], current=0; int direction[3]; + double distance; pc.baud(115200); bt.baud(115200); @@ -74,8 +78,11 @@ tools_section(location, current); mid_section(location, current, direction); mid_section2(location, current, direction); - - + /*while(1){ + bt.printf("IR %f\r\n", IR.getDistance()); + wait_ms(200); + } + */ } @@ -96,7 +103,7 @@ float wall_follow(int side, int direction, int section) { - float location, wavegap=0, set=4; + float location, wavegap=0, set=5; int dir=1; pid1.reset(); @@ -117,12 +124,12 @@ pid1.setSetPoint(set); if(side){ rangeFinderLeft.startMeas(); - wait_ms(20); + wait_ms(38); rangeFinderLeft.getMeas(range); } else{ rangeFinderRight.startMeas(); - wait_ms(20); + wait_ms(38); rangeFinderRight.getMeas(range); pc.printf("%d\r\n",range); } @@ -171,7 +178,7 @@ void wall_follow2(int side, int direction, int section, float location) { int SeeWaveGap = false, dir=1; - float set=4, loc=0; + float set=5, loc=0; pid1.reset(); @@ -181,7 +188,7 @@ leftEncoder.reset(); rightEncoder.reset(); - while(loc + location < 80) { + while(loc + dir*location < 80) { loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; pid1.setInputLimits(0.0, set); @@ -190,12 +197,12 @@ if(side){ rangeFinderLeft.startMeas(); - wait_ms(20); + wait_ms(38); rangeFinderLeft.getMeas(range); } else{ rangeFinderRight.startMeas(); - wait_ms(20); + wait_ms(38); rangeFinderRight.getMeas(range); } @@ -204,7 +211,8 @@ /* If after 20 ms the ultrasonic still sees 20+ cm */ /* then robot is at wave opening */ - pc.printf("range %f\r\n",range); + bt.printf("wall follow 2 range %f\r\n",range); + bt.printf("loc+location = %f\r\n", loc+location); if(range > 20) { motors.stopBothMotors(); bt.printf("wavegap\r\n"); @@ -276,15 +284,7 @@ motors.setMotor0Speed(0.7*MAX_SPEED); //right motors.setMotor1Speed(-0.7*MAX_SPEED); //left } else { - // turn right towards wall - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-MAX_SPEED); //right - motors.setMotor1Speed(MAX_SPEED); //left - while(abs(rightEncoder.getPulses()) < 100); - - motors.stopBothMotors(); - + rightTurn(); motors.setMotor0Speed(-0.7*MAX_SPEED); //right motors.setMotor1Speed(0.7*MAX_SPEED); //left } @@ -320,42 +320,68 @@ rightEncoder.reset(); motors.setMotor0Speed(0.4*127);// right motors.setMotor1Speed(-0.4*127);// left - while(abs(leftEncoder.getPulses())<1075 || rightEncoder.getPulses()<1075); + while(abs(leftEncoder.getPulses())<1000 || rightEncoder.getPulses()<1000); motors.stopBothMotors(); } - -void overBump(int wave){ + +void overBump(int section){ int preLeft=5000, preRight=5000 ; leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(0.15*127); //right motors.setMotor1Speed(0.15*127); //left - while((abs(leftEncoder.getPulses()) < 700 || abs(rightEncoder.getPulses())< 700) && preLeft!=0){ + 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; + } + + 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(); - wait_ms(20); - if(leftEncoder.getPulses() == preLeft && rightEncoder.getPulses()== preRight) preLeft=preRight=0; - } + 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; + } + + motors.stopBothMotors(); - if(wave == FAR){ - while(leftEncoder.getPulses() != preLeft && rightEncoder.getPulses()!= preRight){ + preLeft=preRight=5000 ; + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(MAX_SPEED); //right + motors.setMotor1Speed(MAX_SPEED); //left + + if(section == TOOLS){ + while(IR.getDistance() > 20 ) { + bt.printf("IR %f\r\n", IR.getDistance()); + preLeft=leftEncoder.getPulses(); preRight=rightEncoder.getPulses(); + bt.printf("third while left %d right %d \r\n", preLeft, preRight); wait_ms(20); - } - - motors.stopBothMotors(); + if(leftEncoder.getPulses() == preLeft && rightEncoder.getPulses()== preRight){ + motors.setMotor0Speed(0.4*127); //right + motors.setMotor1Speed(0.4*127); //left + } + } } - - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(0.4*127); //right - motors.setMotor1Speed(0.4*127); //left - while(abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200); - + else while((abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses())< 300)); + motors.stopBothMotors(); + wait(2); + } void tools_section(float* location, float ¤t){ @@ -384,18 +410,8 @@ 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); + overBump(TOOLS); } else{ location[0]= 77; @@ -406,19 +422,12 @@ bt.printf("wavegap = %f\r\n",location[0]); } - + void mid_section(float* location, float ¤t, int* direction){ motors.begin(); alignWithWall(MID); - /* - 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(); - */ + 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); @@ -438,7 +447,7 @@ bt.printf("wavegap2 = %f\r\n",location[1]); leftTurn(); - overBump(FAR); + overBump(TOOLS); // go forward leftEncoder.reset(); rightEncoder.reset(); @@ -452,7 +461,6 @@ 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); @@ -470,10 +478,10 @@ } leftTurn(); - overBump(FAR); + overBump(RIGS); } - + void rig_section(float* location, float ¤t, int* direction, int rig){ -} \ No newline at end of file +} \ No newline at end of file