Diff: main.cpp

Revision:

13:9bad7f74833a

Parent:

12:168cb595f98e

Child:

14:3c8c4efe4786

--- a/main.cpp	Sat Mar 29 22:20:44 2014 +0000
+++ b/main.cpp	Sun Mar 30 00:29:49 2014 +0000
@@ -49,8 +49,7 @@
 //Functions
  
 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 wall_follow2(int side, int direction, int section, float location, int rig);
 void leftTurn(void);
 void slightleft(void);
 void rightTurn(void);
@@ -82,7 +81,7 @@
     tools_section(location, current);
     mid_section(location, current, direction);
     mid_section2(location, current, direction);
-    rig_section(location, current, direction, 1);
+    rig_section(location, current, direction, 3);
     /*while(1) {
         pc.printf("IR %f\r\n", IR.getDistance());
         /*rangeFinderLeft.startMeas();
@@ -186,10 +185,14 @@
  
 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
  
-void wall_follow2(int side, int direction, int section, float location)
+void wall_follow2(int side, int direction, int section, float location, int rig)
 {
-    int dir=1, limit=89;
-    float set=6, loc=0;
+    int dir=1, limit=90;
+    float set=6, loc=0, Rigloc=0;
+    
+    if(rig == 1) Rigloc= 16;
+    else if(rig == 2) Rigloc= 45;
+    else if(rig== 3) Rigloc = 70;
  
     pid1.reset();
  
@@ -205,7 +208,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);
@@ -229,12 +232,19 @@
         }
         
         if(section == RIGS){
-            rangeFinderRight.startMeas();
+            rangeFinderLeft.startMeas();
             wait_ms(20);
-            rangeFinderRight.getMeas(range);
-            if(range< 20){
-                motors.stopBothMotors(); 
-                break;
+            rangeFinderLeft.getMeas(range2);
+            
+            if(range2< 20){
+                if( abs(dir*loc + location - Rigloc) < 10){
+                    //STOP
+                    motors.setMotor0Speed(dir*-0.25*127); //right
+                    motors.setMotor1Speed(dir*-0.25*127); //left
+                    wait_ms(5);
+                    motors.stopBothMotors();
+                    break;
+                }
             }
         }
  
@@ -242,10 +252,21 @@
         //pc.printf("wall follow 2 range %f\r\n",range);
         //pc.printf("loc+location = %f\r\n", loc+location);
         if(range > 20 ) {
-            motors.stopBothMotors(); 
-            pc.printf("wavegap\r\n");
-            // AT WAVE OPENING!!!!
-            break;
+            if(section == RIGS){
+                motors.setMotor0Speed(dir*0.25*127); //right
+                motors.setMotor1Speed(dir*0.25*127); //left
+            }
+            else{
+                //STOP
+                motors.setMotor0Speed(dir*-0.25*127); //right
+                motors.setMotor1Speed(dir*-0.25*127); //left
+                wait_ms(5);
+                motors.stopBothMotors();
+                
+                pc.printf("wavegap\r\n");
+                // AT WAVE OPENING!!!!
+                break;
+            }
         } else {
  
         pid1.setProcessValue(range);
@@ -274,6 +295,7 @@
         }
     }}
     
+    //STOP
     motors.setMotor0Speed(dir*-0.25*127); //right
     motors.setMotor1Speed(dir*-0.25*127); //left
     wait_ms(5);
@@ -396,11 +418,9 @@
     motors.setMotor1Speed(dir*0.25*127); //left
     while(abs(leftEncoder.getPulses()) < pulses || abs(rightEncoder.getPulses()) < pulses);
     
-    leftEncoder.reset();
-    rightEncoder.reset();
     motors.setMotor0Speed(dir*-0.25*127); //right
     motors.setMotor1Speed(dir*-0.25*127); //left
-    while(abs(leftEncoder.getPulses()) < 20 || abs(rightEncoder.getPulses()) < 20);
+    wait_ms(10);
     motors.stopBothMotors();
 }
 
@@ -560,7 +580,7 @@
     rangeFinderLeft.getMeas(range);
  
     if(range < 20) {
-        wall_follow2(LEFT,BACKWARD,TOOLS, current);
+        wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
         pc.printf("wall follow\r\n");
         location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
         current= location[0];
@@ -606,7 +626,7 @@
     wait_ms(100);
  
     pc.printf("mid section current = %f\r\n",current);
-    wall_follow2(LEFT,FORWARD,MID, current);
+    wall_follow2(LEFT,FORWARD,MID, current,0);
     current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     pc.printf("after wf2 current = %f\r\n",current);
     
@@ -618,14 +638,17 @@
     if(range > 20 ) {
         direction[0]= RIGHT;
         location[1]= current;
-        slightMove(FORWARD,100);
+        //slightMove(FORWARD,200);
+        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     } else {
         direction[0]= LEFT;
-        wall_follow2(LEFT,BACKWARD,MID,current);
+        wall_follow2(LEFT,BACKWARD,MID,current,0);
         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);
+            current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
         }  
         
     }
@@ -654,7 +677,7 @@
     alignWithWall(MID);
     pc.printf("midsection 2 alignt with wall mid \r\n");
     
-    wall_follow2(LEFT,FORWARD,MID, current);
+    wall_follow2(LEFT,FORWARD,MID, current,0);
     current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     
     pc.printf("midseection 2 after wf2");
@@ -666,12 +689,13 @@
         direction[1]= RIGHT;
         location[2]= current;
         slightMove(FORWARD,500);
+        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     } else {
         direction[1]= LEFT;
-        wall_follow2(LEFT,BACKWARD,MID,current);
+        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(FORWARD,500);
     }
  
     leftTurn();
@@ -683,33 +707,20 @@
 {
     float loc;
     
-    if(rig == 1) loc= 16;
+    if(rig == 1) loc= 15;
     else if(rig == 2) loc= 45;
-    else loc = 70;
+    else loc = 75;
     
-    alignWithWall(RIGS);
+    rightTurn();
     
     if(current > loc){
-        UntilWall(BACKWARD);
-        current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-        wall_follow2(RIGHT, BACKWARD, RIGS, current);
+        pc.printf("RIG section %f\r\n",current);
+        wall_follow2(RIGHT, BACKWARD, RIGS, current, rig);
         current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-        
-        if((current- loc)>10){
-            wall_follow2(RIGHT, BACKWARD, RIGS, current);
-            current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-        }   
     }
     else{
-        UntilWall(FORWARD);
-        wall_follow2(RIGHT, FORWARD, RIGS, current);
-        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-        
-        if((current- loc)<-10){
-            wall_follow2(RIGHT, FORWARD, RIGS, current);
-            current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-        }   
+        pc.printf("RIG section %f\r\n",current);
+        wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
+        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 
     }
-    
- 
 }
\ No newline at end of file