revise
Dependencies: HCSR04 PID PololuQik2 QEI mbed-rtos Sharp
Diff: main.cpp
- Revision:
- 7:78745a518957
- Parent:
- 6:f5c26372b2d0
- Child:
- 8:11ef93eebe07
diff -r f5c26372b2d0 -r 78745a518957 main.cpp --- a/main.cpp Fri Mar 21 21:26:50 2014 +0000 +++ b/main.cpp Fri Mar 21 22:19:51 2014 +0000 @@ -69,6 +69,7 @@ bt.baud(115200); motors.begin(); + bt.printf("START\r\n"); //Go to tools tools_section(location, current); mid_section(location, current, direction); @@ -128,7 +129,7 @@ if(range > 20) { wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - bt.printf("wavegap %f\r\n",wavegap); + //bt.printf("wavegap %f\r\n",wavegap); // AT WAVE OPENING!!!! motors.setMotor1Speed(dir*0.3*127);//left motors.setMotor0Speed(dir*0.3*127);//right @@ -170,7 +171,7 @@ void wall_follow2(int side, int direction, int section, float location) { int SeeWaveGap = false, dir=1; - float set=4, loc; + float set=4, loc=0; pid1.reset(); @@ -179,9 +180,9 @@ leftEncoder.reset(); rightEncoder.reset(); - - while(loc - location ) { - location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + + while(loc + location < 80) { + 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); @@ -259,7 +260,7 @@ rightEncoder.reset(); motors.setMotor0Speed(-MAX_SPEED); //right motors.setMotor1Speed(-MAX_SPEED); //left - while(abs(leftEncoder.getPulses()) < 350 || abs(rightEncoder.getPulses()) < 350); + while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300); motors.stopBothMotors(); @@ -271,28 +272,29 @@ while(rightEncoder.getPulses() < 100); motors.stopBothMotors(); - } - else{ - // turn right towards wall + + 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()) < 300); - + while(abs(rightEncoder.getPulses()) < 100); + motors.stopBothMotors(); + + motors.setMotor0Speed(-0.7*MAX_SPEED); //right + motors.setMotor1Speed(0.7*MAX_SPEED); //left } - // align using the distance of the wall - motors.setMotor0Speed(0.7*MAX_SPEED); //right - motors.setMotor1Speed(-0.7*MAX_SPEED); //left - usValue = 0; while(1){ rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); - bt.printf("Range %f \t OldValue %f\n\r",range, usValue); + //bt.printf("Range %f \t OldValue %f\n\r",range, usValue); if(range > usValue && usValue != 0 && range < 25){ break; } else { @@ -408,7 +410,6 @@ void mid_section(float* location, float ¤t, int* direction){ motors.begin(); - rightTurn(); alignWithWall(MID); /* leftEncoder.reset();