Revision 21:2936d9566213, committed 2014-04-05

Comitter:

Fairy_Paolina

Date:

Sat Apr 05 03:27:16 2014 +0000

Parent:

20:b9cbaf7566e9

Commit message:

klasdjflj;adlj;fdjlkfdskjlfsdak;ljfasdjkl; ;

Changed in this revision

--- 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();