uyvug
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 24:3369d51f6cbd
- Parent:
- 23:f8e806d1ffcc
- Child:
- 26:7257bd16bc67
--- a/main.cpp Sat Apr 12 20:27:00 2014 +0000 +++ b/main.cpp Sat Apr 12 23:22:33 2014 +0000 @@ -272,6 +272,7 @@ pc.baud(115200); + //Laser Range Finder Initialization lrf_baudCalibration(); @@ -303,7 +304,9 @@ **************************************************/ case START : myled1 = 1; - state = OILRIG1_POS; + current=75; + state = NAVIGATE_WAVES_ROW1; + //state = OILRIG1_POS; break; @@ -1247,7 +1250,7 @@ rightEncoder.reset(); motors.setMotor0Speed(-0.5*127);//right motors.setMotor1Speed(0.5*127);//left - while(abs(leftEncoder.getPulses())<850 || abs(rightEncoder.getPulses())<850); + while(abs(leftEncoder.getPulses())<800 || abs(rightEncoder.getPulses())<800); motors.stopBothMotors(127); } @@ -1322,87 +1325,76 @@ void overBump(int section) { - int preLeft=5000, preRight=5000, out=0; - - motors.begin(); - // slight backwards - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-0.25*127); //right - motors.setMotor1Speed(-0.25*127); //left - while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50); - - motors.stopBothMotors(127); - - //pc.printf("slight backwards\r\n"); + // first set + motors.setMotor1Speed(0.1*127);//left + motors.setMotor0Speed(0.1*127);//right + wait_ms(100); + while(motors.GetMotor0Current()<5 || motors.GetMotor1Current() <5 ); + + while(motors.GetMotor0Current()>5 || motors.GetMotor1Current() >5 ) { + pc.printf("current %f\r\n",IR.getIRDistance()); + pc.printf("left %d \t right %d \r\n", motors.GetMotor0Current(), motors.GetMotor1Current()); + motors.setMotor1Speed(0.3*127);//left + motors.setMotor0Speed(0.3*127);//right + wait_ms(220); + motors.stopBothMotors(127); + wait_ms(10); + } + + motors.setMotor1Speed(0.1*127);//left + motors.setMotor0Speed(0.1*127);//right + wait_ms(100); + while(motors.GetMotor0Current()<5 || motors.GetMotor1Current() <5 ); + + motors.setMotor1Speed(0.3*127);//left + motors.setMotor0Speed(0.3*127);//right + // second set wait_ms(200); - - // Over bump - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(0.33*127); //right - motors.setMotor1Speed(0.33*127); //left - while((abs(leftEncoder.getPulses()) < 900 || abs(rightEncoder.getPulses()) < 900) /*&& preLeft!=0*/ && IR.getIRDistance() >15 ); - - - - //pc.printf("forward \r\n"); - wait_ms(200); - - motors.stopBothMotors(0); - motors.begin(); - - preLeft=preRight=5000 ; - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(.25*127); //right - motors.setMotor1Speed(.25*127); //left - - if(section == TOOLS) { - while(IR.getIRDistance() > 10 && (abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) { - - if(IR.getIRDistance() > 38) break; - - preLeft=leftEncoder.getPulses(); - preRight=rightEncoder.getPulses(); + motors.stopBothMotors(127); + wait_ms(300); + if(section!= RIGS) { + + while(IR.getIRDistance() >30) { + pc.printf("%f\r\n",IR.getIRDistance()); + motors.setMotor1Speed(0.3*127);//left + motors.setMotor0Speed(0.3*127);//right + wait_ms(220); + motors.stopBothMotors(127); wait_ms(200); } - } else if(section == MID || section == MID2) { - if(section == MID2) { - motors.setMotor0Speed(.33*127); //right - motors.setMotor1Speed(.33*127); //left - while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400)); - } - - while( IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) /*&& (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)*/) { - - //if(IR.getIRDistance() > 38) break; - - //preLeft=leftEncoder.getPulses(); - //preRight=rightEncoder.getPulses(); - //wait_ms(200); - } - - } else {// RIGS - while(abs(leftEncoder.getPulses()) < 220 || abs(rightEncoder.getPulses()) < 220); - - // go backwards to line up with bump + + motors.setMotor1Speed(0.1*127);//left + motors.setMotor0Speed(0.1*127);//right + wait_ms(100); + while(motors.GetMotor0Current()<5 || motors.GetMotor1Current() <5); + motors.stopBothMotors(127); + wait_ms(200); + + } else { leftEncoder.reset(); rightEncoder.reset(); - - motors.setMotor0Speed(-.15*127); //right - motors.setMotor1Speed(-.15*127); //left - while((abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && preLeft!=0 ) { - preLeft = leftEncoder.getPulses(); - preRight = rightEncoder.getPulses(); + + + while(leftEncoder.getPulses()<500 || rightEncoder.getPulses()<500) { + motors.setMotor1Speed(0.1*127);//left + motors.setMotor0Speed(0.1*127);//right + wait_ms(220); + if(motors.GetMotor0Current()>5 || motors.GetMotor1Current() >5){ + motors.setMotor1Speed(0.3*127);//left + motors.setMotor0Speed(0.3*127);//right + wait_ms(220); + motors.stopBothMotors(127); + wait_ms(200); + leftEncoder.reset(); + rightEncoder.reset(); + } + motors.stopBothMotors(127); wait_ms(200); - if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0; } - motors.stopBothMotors(127); - motors.begin(); - - return; } + + + motors.stopBothMotors(127); wait_ms(20); @@ -1547,13 +1539,7 @@ //current-=4; //} rightTurn(); - //right turn more - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-0.4*127);// right - motors.setMotor1Speed(0.4*127);// left - while(abs(leftEncoder.getPulses())<25 || abs(rightEncoder.getPulses())<25); - motors.stopBothMotors(127); + //pc.printf("mid section current = %f\r\n",current); wall_follow2(LEFT,FORWARD,MID, current,0); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);