ksdflsjdfljas
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 21:2936d9566213
- Parent:
- 20:b9cbaf7566e9
diff -r b9cbaf7566e9 -r 2936d9566213 main.cpp --- a/main.cpp Fri Apr 04 18:39:38 2014 +0000 +++ b/main.cpp Sat Apr 05 03:27:16 2014 +0000 @@ -1028,8 +1028,8 @@ void wall_follow2(int side, int direction, int section, float location, int rig) { - int dir=1, limit=78, lowlim=4; - float set=7, loc=0, Rigloc=0; + int dir=1, limit=80, lowlim=4; + float set=9, loc=0, Rigloc=0; bool SeeWaveGap = false; if(rig == 1) Rigloc= 16; @@ -1044,10 +1044,10 @@ } else if(direction == FORWARD) lowlim=-20; if(section == TOOLS) { - set= 7; + set= 9; limit = 86; } - else if(section == RIGS) set = 7; + else if(section == RIGS) set = 9; else if(section == RETURN) lowlim=4; else if(section == MID2) limit =85; @@ -1056,7 +1056,7 @@ leftEncoder.reset(); rightEncoder.reset(); - //pc.printf("before %f\r\n", location); + pc.printf("before %f\r\n", location); //pc.printf("dir*loc+location %f\r\n",dir*loc + location ); //pc.printf("limit %d \r\n", limit); @@ -1118,7 +1118,7 @@ SeeWaveGap = false; pid1.setProcessValue(range); pid_return = pid1.compute(); - //pc.printf("Range: %f\n PID: %f\r\n", range, pid_return); + pc.printf("Range: %f\n PID: %f\r\n", range, pid_return); if(pid_return > 0) { if(side) { @@ -1316,7 +1316,7 @@ rightEncoder.reset(); motors.setMotor0Speed(0.5*127);// right motors.setMotor1Speed(-0.5*127);// left - while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100); + while(abs(leftEncoder.getPulses())<1075 || rightEncoder.getPulses()<1075); motors.stopBothMotors(127); } @@ -1338,7 +1338,7 @@ rightEncoder.reset(); motors.setMotor0Speed(-0.4*127);// right motors.setMotor1Speed(0.4*127);// left - while(abs(leftEncoder.getPulses())<50 || abs(rightEncoder.getPulses())<50); + while(abs(leftEncoder.getPulses())<200 || abs(rightEncoder.getPulses())<200); motors.stopBothMotors(127); } @@ -1399,7 +1399,7 @@ rightEncoder.reset(); motors.setMotor0Speed(0.3*127); //right motors.setMotor1Speed(0.3*127); //left - while((abs(leftEncoder.getPulses()) < 900 || abs(rightEncoder.getPulses()) < 900) /*&& preLeft!=0*/ && IR.getIRDistance() >15 ); + while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getIRDistance() >15 ); @@ -1425,9 +1425,9 @@ wait_ms(200); } } else if(section == MID || section == MID2) { - if(section == MID2) while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400)); + if(section == MID2) while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100)); - while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) { + while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) { if(IR.getIRDistance() > 38) break; @@ -1501,7 +1501,7 @@ current= location[0]; //pc.printf("current %f \r\n",current); // go backwards - slightMove(BACKWARD,200); + //slightMove(BACKWARD,200); wait_ms(100); leftTurn(); @@ -1603,6 +1603,7 @@ if(range > 20 ) { direction[0]= RIGHT; location[1]= current; + wait_ms(300); slightMove(FORWARD,100); //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } else { @@ -1614,12 +1615,20 @@ if(location[1] < 18) { slightMove(FORWARD, 75); } - slightMove(BACKWARD,100); + //slightMove(BACKWARD,100); } - + + wait_ms(200); //pc.printf("wavegap2 = %f\r\n",location[1]); - leftTurn(); + //left turn + motors.begin(); + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(0.5*127);// right + motors.setMotor1Speed(-0.5*127);// left + while(abs(leftEncoder.getPulses())<1045 || rightEncoder.getPulses()<1045); + motors.stopBothMotors(127); wait_ms(100); @@ -1641,6 +1650,7 @@ wait_ms(100); rightTurn(); + slightright(); wait_ms(100); @@ -1657,14 +1667,14 @@ if(range > 20 ) { direction[1]= RIGHT; location[2]= current; - slightMove(FORWARD,100); + slightMove(FORWARD,50); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } else { direction[1]= LEFT; wall_follow2(LEFT,BACKWARD,MID,current,0); location[2]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); current=location[2]; - slightMove(BACKWARD,100); + //slightMove(BACKWARD,100); } //LEFT turn @@ -1673,7 +1683,7 @@ rightEncoder.reset(); motors.setMotor0Speed(0.5*127);// right motors.setMotor1Speed(-0.5*127);// left - while(abs(leftEncoder.getPulses())<900 || rightEncoder.getPulses()<900); + while(abs(leftEncoder.getPulses())<950 || rightEncoder.getPulses()<950); motors.stopBothMotors(127); overBump(RIGS); @@ -1689,7 +1699,7 @@ else loc = 75; // Slight forward for turn - slightMove(FORWARD,100); + slightMove(FORWARD,150); wait_ms(100); rightTurn(); //slightright();