ksdflsjdfljas
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 19:9329e9be4c66
- Parent:
- 18:a0ea7ecaf4fe
- Child:
- 20:b9cbaf7566e9
diff -r a0ea7ecaf4fe -r 9329e9be4c66 main.cpp --- a/main.cpp Thu Apr 03 22:41:24 2014 +0000 +++ b/main.cpp Thu Apr 03 23:01:50 2014 +0000 @@ -1006,7 +1006,7 @@ void wall_follow2(int side, int direction, int section, float location, int rig) { int dir=1, limit=86, lowlim=5; - float set=7, loc=0, Rigloc=0; + float set=6, loc=0, Rigloc=0; bool SeeWaveGap = false; if(rig == 1) Rigloc= 16; @@ -1023,7 +1023,8 @@ set= 6; limit = 86; } - if(section == RETURN) lowlim=15; + else if(section == RIGS) set = 3; + else if(section == RETURN) lowlim=15; leftEncoder.reset(); rightEncoder.reset(); @@ -1180,19 +1181,20 @@ motors.stopBothMotors(0); wait(2); - // turn left towards wall + // turn right towards wall leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-MAX_SPEED); //right motors.setMotor1Speed(MAX_SPEED); //left - while(abs(rightEncoder.getPulses()) < 20 || abs(leftEncoder.getPulses()) < 20); + while(abs(rightEncoder.getPulses()) < 50 || abs(leftEncoder.getPulses()) < 50); - motors.stopBothMotors(0); - wait(2); + motors.stopBothMotors(127); +/* wait(2); // turning left motors.setMotor0Speed(-0.9*MAX_SPEED); //right motors.setMotor1Speed(0.9*MAX_SPEED); //left +*/ } else {// MID pc.printf("in mid section align\r\n"); // turn right towards wall @@ -1204,7 +1206,7 @@ usValue = 0; while(1) { - if(section == RIGS) { + if(section == 10) { // CURENTLY NOT USED (WAS RIGS) rangeFinderRight.startMeas(); wait_ms(20); rangeFinderRight.getMeas(range); @@ -1621,6 +1623,11 @@ } alignWithWall(RIGS); + // Go forward until Rig + wall_follow2(RIGHT, FORWARD, RIGS, current, rig); + // line back wheel up with side of rig + slightMove(FORWARD,300); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } void tools_section_return(float* location, float ¤t)