Yeah
Dependencies: HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of NavigationTest by
Diff: main.cpp
- Revision:
- 3:58726d2e11f0
- Parent:
- 2:3d0be48abcf2
- Child:
- 4:f2333e66ec2c
--- a/main.cpp Sat Mar 15 22:37:06 2014 +0000 +++ b/main.cpp Tue Mar 18 17:47:45 2014 +0000 @@ -16,10 +16,15 @@ #define DIST_PER_PULSE (0.01054225722682) #define MTRS_TO_INCH (39.3701) #define MAX_SPEED (0.3*127) -#define PPRL (965) -#define PPRR (1075) +#define PPRL (24) +#define PPRR (24) #define LEFT (1) #define RIGHT (0) +#define FORWARD (1) +#define BACKWARD (0) +#define TOOLS (0) +#define MID (1) +#define RIGS (2) float range, pid_return; void errFunction(void); @@ -39,53 +44,76 @@ //Functions -void wall_follow(int side); -void wall_follow2(int side); +float wall_follow(int side, int direction, int section); +void wall_follow2(int side, int direction); void wall_follow3(int ¤tLocation, int &WaveOpening); void leftTurn(void); void rightTurn(void); void us_distance(void); +void tools_section(float* location, float ¤t); +void overBump(void); //Variables int main(void) { - float location=0; + float location[3], current=0; + int direction[3]; pc.baud(115200); bt.baud(115200); motors.begin(); - //motors.setMotor0Speed(MAX_SPEED); //right - //motors.setMotor1Speed(MAX_SPEED); //left - - //motors.stopBothMotors(); - - - //wall_follow(RIGHT); - leftEncoder.reset(); - rightEncoder.reset(); - wall_follow2(LEFT); - location=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2; - - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(MAX_SPEED); //right - motors.setMotor1Speed(MAX_SPEED); //left - while(leftEncoder.getPulses()<300 || rightEncoder.getPulses()<300); - motors.stopBothMotors(); - - leftTurn(); 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())/(44*PPRL) + abs(rightEncoder.getPulses())/(44*PPRR))/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); + /* + //////////////////////////////// without predefined wavegaps////////////////////////////////////////////// + current=0; + if(location[0]< 75){ + turnRight(); + current=wall_follow(LEFT,FORWARD); + if(current == 0)turnLeft(); + else{ + direction[0]= RIGHT; + turnLeft(); + overBump(); + } + } + else if(location[0]>=75 || current == 0){ + turnLeft(); + wall_follow2(RIGHT,FORWARD); + } + + + + + + + // left or right + + + location[1]=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2; + + + + + + + leftTurn(); - wall_follow2(RIGHT); + //wall_follow2(RIGHT); rightTurn(); @@ -96,7 +124,7 @@ // leftTurn(); // wait(1); // rightTurn(); - +*/ } @@ -115,13 +143,25 @@ } } -void wall_follow(int side) +float wall_follow(int side, int direction, int section) { - while(1) { - - pid1.setInputLimits(0, 5.0); + float location, wavegap; + int dir=1, set=5; + + 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; + + while(location< 75) { + location=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2; + + pid1.setInputLimits(0, set); pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); - pid1.setSetPoint(5.0); + pid1.setSetPoint(set); if(side){ rangeFinderLeft.startMeas(); wait_ms(20); @@ -134,40 +174,51 @@ pc.printf("%d\r\n",range); } + if(range > 20) { + wavegap=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2; + bt.printf("wavegap %f\r\n",wavegap); + // AT WAVE OPENING!!!! + } + pid1.setProcessValue(range); pid_return = pid1.compute(); if(pid_return > 0) { if(side){ - motors.setMotor0Speed(MAX_SPEED - pid_return);//right - motors.setMotor1Speed(MAX_SPEED);//left + motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right + motors.setMotor1Speed(dir*MAX_SPEED);//left } else{ - motors.setMotor1Speed(MAX_SPEED - pid_return);//left - motors.setMotor0Speed(MAX_SPEED);//right + motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left + motors.setMotor0Speed(dir*MAX_SPEED);//right } }else if(pid_return < 0) { if(side){ - motors.setMotor0Speed(MAX_SPEED);//right - motors.setMotor1Speed(MAX_SPEED + pid_return);//left + motors.setMotor0Speed(dir*MAX_SPEED);//right + motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left } else{ - motors.setMotor1Speed(MAX_SPEED);//left - motors.setMotor0Speed(MAX_SPEED + pid_return);//right + motors.setMotor1Speed(dir*MAX_SPEED);//left + motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right } }else { - motors.setMotor0Speed(MAX_SPEED);//right - motors.setMotor1Speed(MAX_SPEED);//left + motors.setMotor0Speed(dir*MAX_SPEED);//right + motors.setMotor1Speed(dir*MAX_SPEED);//left } } + return wavegap; } /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ -void wall_follow2(int side) +void wall_follow2(int side, int direction) { - int SeeWaveGap = false; - int count=0; + int SeeWaveGap = false, count=0, dir=1; + + if(direction == BACKWARD) dir=-1; + + leftEncoder.reset(); + rightEncoder.reset(); while(1) { @@ -188,7 +239,7 @@ /*************CHECK FOR WAVE OPENING*****************/ - /* If after 60 ms the ultrasonic still sees 20+ cm */ + /* If after 20 ms the ultrasonic still sees 20+ cm */ /* then robot is at wave opening */ pc.printf("range %f\r\n",range); @@ -205,25 +256,25 @@ if(pid_return > 0) { if(side){ - motors.setMotor0Speed(MAX_SPEED - pid_return);//right - motors.setMotor1Speed(MAX_SPEED);//left + motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right + motors.setMotor1Speed(dir*MAX_SPEED);//left } else{ - motors.setMotor1Speed(MAX_SPEED - pid_return);//left - motors.setMotor0Speed(MAX_SPEED);//right + motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left + motors.setMotor0Speed(dir*MAX_SPEED);//right } }else if(pid_return < 0) { if(side){ - motors.setMotor0Speed(MAX_SPEED);//right - motors.setMotor1Speed(MAX_SPEED + pid_return);//left + motors.setMotor0Speed(dir*MAX_SPEED);//right + motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left } else{ - motors.setMotor1Speed(MAX_SPEED);//left - motors.setMotor0Speed(MAX_SPEED + pid_return);//right + motors.setMotor1Speed(dir*MAX_SPEED);//left + motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right } } else { - motors.setMotor0Speed(MAX_SPEED); - motors.setMotor1Speed(MAX_SPEED); + motors.setMotor0Speed(dir*MAX_SPEED); + motors.setMotor1Speed(dir*MAX_SPEED); } } } @@ -303,3 +354,52 @@ while(leftEncoder.getPulses()>-1200 || rightEncoder.getPulses()<1200); motors.stopBothMotors(); } + +void overBump(void){ + + 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); + 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; + 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); + // 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(); + 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; + } + + leftTurn(); + + //Go over + overBump(); + */ +}