Yeah
Dependencies: HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of NavigationTest by
Diff: main.cpp
- Revision:
- 4:f2333e66ec2c
- Parent:
- 3:58726d2e11f0
- Child:
- 5:70ccef3734ae
--- a/main.cpp Tue Mar 18 17:47:45 2014 +0000 +++ b/main.cpp Tue Mar 18 23:11:28 2014 +0000 @@ -16,8 +16,7 @@ #define DIST_PER_PULSE (0.01054225722682) #define MTRS_TO_INCH (39.3701) #define MAX_SPEED (0.3*127) -#define PPRL (24) -#define PPRR (24) +#define PPR (4331/4) #define LEFT (1) #define RIGHT (0) #define FORWARD (1) @@ -37,15 +36,15 @@ HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR ); PID pid1(15.0,0.0,4.0,0.02); PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc); -QEI rightEncoder(p17,p18,NC,PPRR,QEI::X4_ENCODING); -QEI leftEncoder(p16,p15,NC,PPRR,QEI::X4_ENCODING); +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); +void wall_follow2(int side, int direction, int section); void wall_follow3(int ¤tLocation, int &WaveOpening); void leftTurn(void); void rightTurn(void); @@ -65,20 +64,18 @@ motors.begin(); - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(MAX_SPEED); //right - motors.setMotor1Speed(MAX_SPEED); //left + //leftEncoder.reset(); + //rightEncoder.reset(); + //motors.setMotor0Speed(MAX_SPEED); //right + //motors.setMotor1Speed(MAX_SPEED); //left - while((abs(leftEncoder.getPulses())/(44*PPRL) + abs(rightEncoder.getPulses())/(44*PPRR))/2 < 3); + //while((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2 < 3) + - //while(((abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2 -3) < 20); - - motors.stopBothMotors(); //Go to tools - //tools_section(location, current); - /* + tools_section(location, current); + /* //////////////////////////////// without predefined wavegaps////////////////////////////////////////////// current=0; if(location[0]< 75){ @@ -104,7 +101,7 @@ // left or right - location[1]=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2; + location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; @@ -148,16 +145,18 @@ float location, wavegap; int dir=1, set=5; + pid1.reset(); + if(direction == BACKWARD) dir=-1; if(section == TOOLS)set= 9; leftEncoder.reset(); rightEncoder.reset(); - location=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2; + location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; while(location< 75) { - location=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2; + location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; pid1.setInputLimits(0, set); pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); @@ -175,7 +174,7 @@ } if(range > 20) { - wavegap=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2; + wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; bt.printf("wavegap %f\r\n",wavegap); // AT WAVE OPENING!!!! } @@ -211,11 +210,14 @@ /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ -void wall_follow2(int side, int direction) +void wall_follow2(int side, int direction, int section) { - int SeeWaveGap = false, count=0, dir=1; + int SeeWaveGap = false, count=0, dir=1, set=5; + + pid1.reset(); if(direction == BACKWARD) dir=-1; + if(section == TOOLS)set= 9; leftEncoder.reset(); rightEncoder.reset(); @@ -359,47 +361,56 @@ 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); + + leftEncoder.reset(); + rightEncoder.reset(); motors.setMotor0Speed(MAX_SPEED); //right motors.setMotor1Speed(MAX_SPEED); //left - while(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR)/2 <30); + while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <3); + 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/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2; + 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(); ////////////////////////////////////////// determine shape and pick up tool /////////////////////////////////////////////////////// - /* + if(current >location[0]){ - wall_follow2(LEFT,BACKWARD); + wall_follow2(LEFT,BACKWARD,TOOLS); + wait_ms(500); // forward - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(MAX_SPEED); //right - motors.setMotor1Speed(MAX_SPEED); //left - while(leftEncoder.getPulses()<300 || rightEncoder.getPulses()<300); - motors.stopBothMotors(); - current+=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2; - } - else{ - wall_follow2(LEFT,FORWARD); - // backward - leftEncoder.reset(); + /* 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(); - current-=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2; + wait_ms(500);*/ + current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + } + else{/* + wall_follow2(LEFT,FORWARD); + // backward + 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(); + current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;*/ } leftTurn(); //Go over overBump(); - */ + }