Diff: main.cpp

Revision:

12:168cb595f98e

Parent:

11:d67a3958127a

Child:

13:9bad7f74833a

--- a/main.cpp	Sat Mar 29 20:21:55 2014 +0000
+++ b/main.cpp	Sat Mar 29 22:20:44 2014 +0000
@@ -48,7 +48,7 @@
  
 //Functions
  
-float wall_follow(int side, int direction, int section);
+void wall_follow(int side, int direction, int section);
 void wall_follow2(int side, int direction, int section, float location);
 void wall_follow3(int &currentLocation, int &WaveOpening);
 void leftTurn(void);
@@ -82,6 +82,7 @@
     tools_section(location, current);
     mid_section(location, current, direction);
     mid_section2(location, current, direction);
+    rig_section(location, current, direction, 1);
     /*while(1) {
         pc.printf("IR %f\r\n", IR.getDistance());
         /*rangeFinderLeft.startMeas();
@@ -117,9 +118,9 @@
     }
 }
  
-float wall_follow(int side, int direction, int section)
+void wall_follow(int side, int direction, int section)
 {
-    float location, wavegap=0, set=6;
+    float location, set=6;
     int dir=1;
 
     pid1.reset();
@@ -134,7 +135,6 @@
  
     while(location< 69) {
         location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-        pc.printf("LOCATION %f\r\n", location);
  
         pid1.setInputLimits(0, set);
         pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
@@ -151,7 +151,6 @@
         }
  
         if(range > 20) {
-            wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
             //pc.printf("wavegap %f\r\n",wavegap);
             // AT WAVE OPENING!!!!
             motors.setMotor1Speed(dir*0.25*127);//left
@@ -183,14 +182,13 @@
             }
         }
     }
-    return wavegap;
 }
  
 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
  
 void wall_follow2(int side, int direction, int section, float location)
 {
-    int dir=1, limit=90;
+    int dir=1, limit=89;
     float set=6, loc=0;
  
     pid1.reset();
@@ -276,11 +274,9 @@
         }
     }}
     
-    leftEncoder.reset();
-    rightEncoder.reset();
     motors.setMotor0Speed(dir*-0.25*127); //right
     motors.setMotor1Speed(dir*-0.25*127); //left
-    while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50);
+    wait_ms(5);
     motors.stopBothMotors();
 }
  
@@ -320,8 +316,13 @@
         motors.setMotor0Speed(0.9*MAX_SPEED); //right
         motors.setMotor1Speed(-0.9*MAX_SPEED); //left
            
-    }else {
-        //rightTurn();
+    }else if( section == RIGS){
+        rightTurn();
+        // turning right towards wall
+        motors.setMotor0Speed(-0.9*MAX_SPEED); //right
+        motors.setMotor1Speed(0.9*MAX_SPEED); //left
+    }
+    else {
         pc.printf("in mid section align\r\n");
         // turn right towards wall
         rightTurn();
@@ -333,9 +334,16 @@
 
     usValue = 0;
     while(1) {
-        rangeFinderLeft.startMeas();
-        wait_ms(20);
-        rangeFinderLeft.getMeas(range);
+        if(section == RIGS){
+            rangeFinderRight.startMeas();
+            wait_ms(20);
+            rangeFinderRight.getMeas(range);
+        }
+        else{
+            rangeFinderLeft.startMeas();
+            wait_ms(20);
+            rangeFinderLeft.getMeas(range);
+        }
         pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
         if(range > usValue && usValue != 0 && range < 25) {
             break;
@@ -353,7 +361,7 @@
     rightEncoder.reset();
     motors.setMotor0Speed(-0.5*127);//right
     motors.setMotor1Speed(0.5*127);//left
-    while(abs(leftEncoder.getPulses())<1100 || abs(rightEncoder.getPulses())<1100);
+    while(abs(leftEncoder.getPulses())<950 || abs(rightEncoder.getPulses())<950);
     motors.stopBothMotors();
 }
  
@@ -392,7 +400,7 @@
     rightEncoder.reset();
     motors.setMotor0Speed(dir*-0.25*127); //right
     motors.setMotor1Speed(dir*-0.25*127); //left
-    while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50);
+    while(abs(leftEncoder.getPulses()) < 20 || abs(rightEncoder.getPulses()) < 20);
     motors.stopBothMotors();
 }
 
@@ -437,9 +445,9 @@
     
     leftEncoder.reset();
     rightEncoder.reset();
-    motors.setMotor0Speed(0.3*127); //right
-    motors.setMotor1Speed(0.3*127); //left
-    while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) && preLeft!=0 && IR.getDistance() >20 ){
+    motors.setMotor0Speed(0.35*127); //right
+    motors.setMotor1Speed(0.35*127); //left
+    while((abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses()) < 1000) && preLeft!=0 && IR.getDistance() >15 ){
        preLeft=leftEncoder.getPulses();
        preRight=rightEncoder.getPulses();
        wait_ms(200);
@@ -498,7 +506,7 @@
         }
     }
     else if( section == MID ){
-        while(IR.getDistance() > 20 && (abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)){
+        while(IR.getDistance() > 15 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)){
             
             if(IR.getDistance() > 38) break;
             
@@ -508,7 +516,7 @@
         }
             
     }
-    else while(abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200);
+    else while(abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100);
  
     motors.stopBothMotors();
     
@@ -573,7 +581,6 @@
  
         wait_ms(100);
         leftTurn();
-        slightleft();
         overBump(TOOLS);
     } else {
         pc.printf("else greater than 20\r\n");
@@ -611,29 +618,24 @@
     if(range > 20 ) {
         direction[0]= RIGHT;
         location[1]= current;
+        slightMove(FORWARD,100);
     } else {
         direction[0]= LEFT;
         wall_follow2(LEFT,BACKWARD,MID,current);
         location[1]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        current= location[1];
         if(location[1] < 18){
             slightMove(FORWARD, 50);
         }  
+        
     }
- 
+    
     pc.printf("wavegap2 = %f\r\n",location[1]);
     leftTurn();
-    slightleft();
     
     wait_ms(100);
     
     overBump(MID);
-    // go forward
-   /* leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(0.2*127); //right
-    motors.setMotor1Speed(0.2*127); //left
-    while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
-    motors.stopBothMotors();*/
  
 }
  
@@ -651,8 +653,10 @@
     
     alignWithWall(MID);
     pc.printf("midsection 2 alignt with wall mid \r\n");
+    
     wall_follow2(LEFT,FORWARD,MID, current);
     current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+    
     pc.printf("midseection 2 after wf2");
     rangeFinderLeft.startMeas();
     wait_ms(20);
@@ -661,14 +665,16 @@
     if(range > 20 ) {
         direction[1]= RIGHT;
         location[2]= current;
+        slightMove(FORWARD,500);
     } else {
         direction[1]= LEFT;
         wall_follow2(LEFT,BACKWARD,MID,current);
         location[2]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        current=location[2];
+        slightMove(BACKWARD,100);
     }
  
     leftTurn();
-    slightleft();
     overBump(RIGS);
     pc.printf("overbump rigs\r\n");
 }
@@ -678,11 +684,10 @@
     float loc;
     
     if(rig == 1) loc= 16;
-    else if(rig == 2) loc= 37;
-    else loc = 58;
+    else if(rig == 2) loc= 45;
+    else loc = 70;
     
-    rightTurn();
-    
+    alignWithWall(RIGS);
     
     if(current > loc){
         UntilWall(BACKWARD);