Diff: main.cpp

Revision:

11:d67a3958127a

Parent:

10:c57f6a5042d7

Child:

12:168cb595f98e

--- a/main.cpp	Thu Mar 27 22:40:22 2014 +0000
+++ b/main.cpp	Sat Mar 29 20:21:55 2014 +0000
@@ -26,7 +26,6 @@
 #define TOOLS          (0)
 #define MID            (1)
 #define RIGS           (2)
-#define FIRST_WAVE     (0)
 #define FAR            (1)
  
  
@@ -55,6 +54,7 @@
 void leftTurn(void);
 void slightleft(void);
 void rightTurn(void);
+void slightMove(int direction, float pulses);
 void us_distance(void);
 void tools_section(float* location, float &current);
 void mid_section(float* location, float &current, int* direction);
@@ -69,30 +69,33 @@
  
 int main(void)
 {
-    float location[3], current=0;
+    float location[3], current=4;
     int direction[3];
     double distance;
  
     pc.baud(115200);
-    bt.baud(115200);
+    pc.baud(115200);
     motors.begin();
  
-    bt.printf("START\r\n");
+    pc.printf("START\r\n");
     //Go to tools
     tools_section(location, current);
     mid_section(location, current, direction);
-    //mid_section2(location, current, direction);
-   /* while(1) {
-        //bt.printf("IR %f\r\n", US.getDistance());
-        rangeFinderLeft.startMeas();
-        wait_ms(20);
+    mid_section2(location, current, direction);
+    /*while(1) {
+        pc.printf("IR %f\r\n", IR.getDistance());
+        /*rangeFinderLeft.startMeas();
+        rangeFinderRight.startMeas();
+        wait_ms(38);
         rangeFinderLeft.getMeas(range);
-        bt.printf("Range = %f\n\r", range);
+        rangeFinderRight.getMeas(range); 
+        pc.printf("leftRange = %f\n\r", range);
+        pc.printf("rightRange = %f\n\r", range);
  
         wait_ms(200);
     }*/
  
- 
+ //wall_follow2(LEFT,FORWARD,MID,0);
     //leftTurn();
     //rightTurn();
  
@@ -116,9 +119,9 @@
  
 float wall_follow(int side, int direction, int section)
 {
-    float location, wavegap=0, set=5;
+    float location, wavegap=0, set=6;
     int dir=1;
- 
+
     pid1.reset();
  
     if(direction == BACKWARD) dir=-1;
@@ -129,29 +132,30 @@
  
     location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
  
-    while(location< 73) {
+    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);
         pid1.setSetPoint(set);
         if(side) {
             rangeFinderLeft.startMeas();
-            wait_ms(38);
+            wait_ms(20);
             rangeFinderLeft.getMeas(range);
         } else {
             rangeFinderRight.startMeas();
-            wait_ms(38);
+            wait_ms(20);
             rangeFinderRight.getMeas(range);
             pc.printf("%d\r\n",range);
         }
  
         if(range > 20) {
             wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-            //bt.printf("wavegap %f\r\n",wavegap);
+            //pc.printf("wavegap %f\r\n",wavegap);
             // AT WAVE OPENING!!!!
-            motors.setMotor1Speed(dir*0.4*127);//left
-            motors.setMotor0Speed(dir*0.4*127);//right
+            motors.setMotor1Speed(dir*0.25*127);//left
+            motors.setMotor0Speed(dir*0.25*127);//right
         } else {
  
             pid1.setProcessValue(range);
@@ -186,23 +190,31 @@
  
 void wall_follow2(int side, int direction, int section, float location)
 {
-    bool SeeWaveGap = false, dir=1, limit=83;
-    float set=5, loc=0;
+    int dir=1, limit=90;
+    float set=6, loc=0;
  
     pid1.reset();
  
-    if(direction == BACKWARD) dir=-1;
+    if(direction == BACKWARD){
+         dir=-1;
+         limit = 100;
+    }
     if(section == TOOLS){
-        set= 5;
-        limit = 78;
+        set= 6;
+        limit = 89;
     }
 
-    
     leftEncoder.reset();
     rightEncoder.reset();
- 
-    while(dir*loc + location <= limit) {
+    
+    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);
+    
+    while((dir*loc + location <= limit) && (dir*loc + location >= 5)) {
         loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+        pc.printf("loc %f \r\n", loc);
  
         pid1.setInputLimits(0.0, set);
         pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
@@ -210,17 +222,17 @@
  
         if(side) {
             rangeFinderLeft.startMeas();
-            wait_ms(38);
+            wait_ms(20);
             rangeFinderLeft.getMeas(range);
-        } else {
+        } else{
             rangeFinderRight.startMeas();
-            wait_ms(38);
+            wait_ms(20);
             rangeFinderRight.getMeas(range);
         }
         
         if(section == RIGS){
             rangeFinderRight.startMeas();
-            wait_ms(38);
+            wait_ms(20);
             rangeFinderRight.getMeas(range);
             if(range< 20){
                 motors.stopBothMotors(); 
@@ -229,23 +241,18 @@
         }
  
  
-        //bt.printf("wall follow 2 range %f\r\n",range);
-        //bt.printf("loc+location = %f\r\n", loc+location);
-        if(range > 20 && !SeeWaveGap) {
-                motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
-                motors.setMotor1Speed(dir*MAX_SPEED);//left
-                wait_ms(30);
-                SeeWaveGap = true;
-         } else if(range > 20 && SeeWaveGap){
+        //pc.printf("wall follow 2 range %f\r\n",range);
+        //pc.printf("loc+location = %f\r\n", loc+location);
+        if(range > 20 ) {
             motors.stopBothMotors(); 
-            bt.printf("wavegap\r\n");
+            pc.printf("wavegap\r\n");
             // AT WAVE OPENING!!!!
             break;
         } else {
  
         pid1.setProcessValue(range);
         pid_return = pid1.compute();
-        //bt.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) {
@@ -268,6 +275,12 @@
             motors.setMotor1Speed(dir*MAX_SPEED);
         }
     }}
+    
+    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);
     motors.stopBothMotors();
 }
  
@@ -277,6 +290,7 @@
     float usValue = 0;
  
     if(section == TOOLS) {
+        pc.printf("tools section align\r\n");
         // turn at an angle
         leftEncoder.reset();
         rightEncoder.reset();
@@ -288,10 +302,9 @@
         //go backwards toward wall
         leftEncoder.reset();
         rightEncoder.reset();
-        motors.setMotor0Speed(-MAX_SPEED); //right
-        motors.setMotor1Speed(-MAX_SPEED); //left
+        motors.setMotor0Speed(-0.25*127); //right
+        motors.setMotor1Speed(-0.25*127); //left
         while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
- 
         motors.stopBothMotors();
  
         // turn left towards wall
@@ -299,24 +312,31 @@
         rightEncoder.reset();
         motors.setMotor0Speed(MAX_SPEED); //right
         motors.setMotor1Speed(-MAX_SPEED); //left
-        while(rightEncoder.getPulses() < 10 || abs(leftEncoder.getPulses()) < 10);
+        while(rightEncoder.getPulses() < 100 || abs(leftEncoder.getPulses()) < 100);
  
         motors.stopBothMotors();
- 
-        motors.setMotor0Speed(0.7*MAX_SPEED); //right
-        motors.setMotor1Speed(-0.7*MAX_SPEED); //left
-    } else {
+        
+        // turning left
+        motors.setMotor0Speed(0.9*MAX_SPEED); //right
+        motors.setMotor1Speed(-0.9*MAX_SPEED); //left
+           
+    }else {
+        //rightTurn();
+        pc.printf("in mid section align\r\n");
+        // turn right towards wall
         rightTurn();
-        motors.setMotor0Speed(-0.7*MAX_SPEED); //right
-        motors.setMotor1Speed(0.7*MAX_SPEED); //left
+        
+        // turning left towards wall
+        motors.setMotor0Speed(0.9*MAX_SPEED); //right
+        motors.setMotor1Speed(-0.9*MAX_SPEED); //left
     }
- 
+
     usValue = 0;
     while(1) {
         rangeFinderLeft.startMeas();
         wait_ms(20);
         rangeFinderLeft.getMeas(range);
-        //bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
+        pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
         if(range > usValue && usValue != 0 && range < 25) {
             break;
         } else {
@@ -333,20 +353,12 @@
     rightEncoder.reset();
     motors.setMotor0Speed(-0.5*127);//right
     motors.setMotor1Speed(0.5*127);//left
-    while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900);
+    while(abs(leftEncoder.getPulses())<1100 || abs(rightEncoder.getPulses())<1100);
     motors.stopBothMotors();
 }
  
 void leftTurn(void)
 {
-    /*
-    leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(0.4*MAX_SPEED); //right
-    motors.setMotor1Speed(-MAX_SPEED); //left
-    while(abs(leftEncoder.getPulses())<2500);
-    motors.stopBothMotors();
-    */
     motors.begin();
     leftEncoder.reset();
     rightEncoder.reset();
@@ -362,7 +374,25 @@
     rightEncoder.reset();
     motors.setMotor0Speed(0.5*127);// right
     motors.setMotor1Speed(-0.5*127);// left
-    while(abs(leftEncoder.getPulses())<100 || rightEncoder.getPulses()<100);
+    while(abs(leftEncoder.getPulses())<90 || rightEncoder.getPulses()<90);
+    motors.stopBothMotors();
+}
+void slightMove(int direction, float pulses){
+    int dir=1;
+    
+    if(direction == BACKWARD) dir= -1;
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
+    motors.setMotor0Speed(dir*0.25*127); //right
+    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()) < 50 || abs(rightEncoder.getPulses()) < 50);
     motors.stopBothMotors();
 }
 
@@ -394,44 +424,33 @@
     int preLeft=5000, preRight=5000, out=0;
     
     motors.begin();
-    
+    // slight backwards 
     leftEncoder.reset();
     rightEncoder.reset();
-    motors.setMotor0Speed(-0.2*127); //right
-    motors.setMotor1Speed(-0.2*127); //left
-    while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses())< 50);
+    motors.setMotor0Speed(-0.25*127); //right
+    motors.setMotor1Speed(-0.25*127); //left
+    while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50);
     motors.stopBothMotors();
     
+    pc.printf("slight backwards\r\n");
+    wait_ms(200);
+    
     leftEncoder.reset();
     rightEncoder.reset();
-    motors.setMotor0Speed(0.2*127); //right
-    motors.setMotor1Speed(0.2*127); //left
-    while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){
+    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 ){
        preLeft=leftEncoder.getPulses();
        preRight=rightEncoder.getPulses();
-       wait_ms(100);
-       //bt.printf(" first while left %d right %d \r\n", preLeft, preRight);
+       wait_ms(200);
        if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
     }
     
+    pc.printf("forward \r\n");
+    wait_ms(200);
+ /*   
     motors.stopBothMotors();
     motors.begin();
-    wait(2);
-    /*
-        motors.stopBothMotors();
-        motors.setMotor0Speed(0.15*127); //right
-        motors.setMotor1Speed(0.15*127); //left
-        preLeft=preRight=5000 ;
-        leftEncoder.reset();
-        rightEncoder.reset();
-     */
-//   while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){
-    /*        preLeft=leftEncoder.getPulses();
-            preRight=rightEncoder.getPulses();
-            bt.printf("second while left %d right %d \r\n", preLeft, preRight);
-            wait_ms(200);
-            if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
-        }*/
         
     leftEncoder.reset();
     rightEncoder.reset();
@@ -452,12 +471,14 @@
         if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) {
             motors.setMotor0Speed(0.4*127); //right
             motors.setMotor1Speed(0.4*127); //left
+            wait_ms(50);
+            out=1;
         }
         if(abs(leftEncoder.getPulses()) >1000 || abs(leftEncoder.getPulses())>1000) out=1;
     }
- 
+    */
+    
     motors.stopBothMotors();
-    wait(2);
     motors.begin();
     
     preLeft=preRight=5000 ;
@@ -465,19 +486,41 @@
     rightEncoder.reset();
     motors.setMotor0Speed(.25*127); //right
     motors.setMotor1Speed(.25*127); //left
+    
+    if(section == TOOLS){
+        while(IR.getDistance() > 10 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)){
+            
+            if(IR.getDistance() > 38) break;
+            
+            preLeft=leftEncoder.getPulses();
+            preRight=rightEncoder.getPulses();
+            wait_ms(200);
+        }
+    }
+    else if( section == MID ){
+        while(IR.getDistance() > 20 && (abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)){
+            
+            if(IR.getDistance() > 38) break;
+            
+            preLeft=leftEncoder.getPulses();
+            preRight=rightEncoder.getPulses();
+            wait_ms(200);
+        }
+            
+    }
+    else while(abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200);
  
-    if(section == TOOLS || section == MID) {
-        while(IR.getDistance() > 20 ) {
-            //bt.printf("IR %f\r\n", IR.getDistance());
-            //bt.printf("third while left %d right %d \r\n", preLeft, preRight);
-        }
-    } else while((abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses())< 200));
- 
+    motors.stopBothMotors();
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
+    
     motors.setMotor0Speed(-.25*127); //right
     motors.setMotor1Speed(-.25*127); //left
-    wait_ms(10);
+    while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10));
+    
     motors.stopBothMotors();
-    wait(2);
+    wait_ms(50);
     motors.begin();
  
 }
@@ -485,36 +528,24 @@
 void tools_section(float* location, float &current)
 {
     wall_follow(LEFT,FORWARD, TOOLS);
-    // current position in reference to the starting position
-    current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-    bt.printf("current %f \r\n",current);
- 
-    
-    motors.setMotor0Speed(-.2*127); //right
-    motors.setMotor1Speed(-.2*127); //left
-    wait_ms(5);
+    motors.setMotor0Speed(-.3*127); //right
+    motors.setMotor1Speed(-.3*127); //left
+    wait_ms(10);
     motors.stopBothMotors();
-    
-    motors.setMotor0Speed(.2*127); //right
-    motors.setMotor1Speed(.2*127); //left
-    while(IR.getDistance()>6);
-    
-    motors.setMotor0Speed(-.2*127); //right
-    motors.setMotor1Speed(-.2*127); //left
-    wait_ms(5);
-    motors.stopBothMotors();
-    
- 
+    // current position in reference to the starting position
+    current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+    pc.printf("current %f \r\n",current);
+     
     //Tool aquiring
     wait(2);
     // After tool is aquired
  
     alignWithWall(TOOLS);
-    
+    pc.printf("align\r\n");
     wait_ms(100);
     
-    wall_follow2(LEFT,FORWARD,MID, current);
-    current= 78;
+    //wall_follow2(LEFT,FORWARD,MID, current);
+    //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     
     rangeFinderLeft.startMeas();
     wait_ms(20);
@@ -522,105 +553,124 @@
  
     if(range < 20) {
         wall_follow2(LEFT,BACKWARD,TOOLS, current);
-        
+        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];
-        
+        pc.printf("current %f \r\n",current);
+        // go backwards
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(-MAX_SPEED); //right
         motors.setMotor1Speed(-MAX_SPEED); //left
         while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
-        
+        // hard stop
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(MAX_SPEED); //right
         motors.setMotor1Speed(MAX_SPEED); //left
-        while(abs(leftEncoder.getPulses()) < 40 || abs(rightEncoder.getPulses())< 40);
+        while(abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses())< 10);
         motors.stopBothMotors();
  
-        wait_ms(500);
+        wait_ms(100);
         leftTurn();
         slightleft();
         overBump(TOOLS);
     } else {
-        location[0]= 77;
+        pc.printf("else greater than 20\r\n");
+        location[0]= current;
         leftTurn();
-        wait_ms(20);
-        overBump(FIRST_WAVE);
+        overBump(TOOLS);
     }
  
-    bt.printf("First Wavegap = %f\r\n",location[0]);
+    pc.printf("First Wavegap = %f\r\n",location[0]);
 }
  
 void mid_section(float* location, float &current, int* direction)
 {
- 
     motors.begin();
     
-    if(IR.getDistance() > 20){
+    if(IR.getDistance() > 38){
         direction[0]= STRAIGHT;
+        overBump(MID);
         return;
     }
-    
+    pc.printf("before align with wall \r\n");
     alignWithWall(MID);
+    wait_ms(100);
  
-    bt.printf("mid section current = %f\r\n",current);
+    pc.printf("mid section current = %f\r\n",current);
     wall_follow2(LEFT,FORWARD,MID, current);
-    current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-    bt.printf("after wf2 current = %f\r\n",current);
- 
-    if(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);
+    
+    wait_ms(500);
+    rangeFinderLeft.startMeas();
+    wait_ms(20);
+    rangeFinderLeft.getMeas(range);
+    
+    if(range > 20 ) {
         direction[0]= RIGHT;
-        current+= location[0];
         location[1]= current;
     } else {
-        current=location[0];
         direction[0]= LEFT;
         wall_follow2(LEFT,BACKWARD,MID,current);
-        location[1]= location[0]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        location[1]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        if(location[1] < 18){
+            slightMove(FORWARD, 50);
+        }  
     }
  
-    bt.printf("wavegap2 = %f\r\n",location[1]);
+    pc.printf("wavegap2 = %f\r\n",location[1]);
     leftTurn();
-    overBump(TOOLS);
+    slightleft();
+    
+    wait_ms(100);
+    
+    overBump(MID);
     // go forward
-    leftEncoder.reset();
+   /* 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();
+    motors.stopBothMotors();*/
  
 }
  
 void mid_section2(float* location, float &current, int* direction)
 {
- 
     motors.begin();
     
-    if(IR.getDistance() > 20){
+    pc.printf("mid section 2\r\n");
+    
+    if(IR.getDistance() > 38){
         direction[0]= STRAIGHT;
+        overBump(RIGS);
         return;
     }
     
     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);
- 
-    if(current != 0) {
+    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);
+    rangeFinderLeft.getMeas(range);
+    
+    if(range > 20 ) {
         direction[1]= RIGHT;
-        current+= location[1];
         location[2]= current;
     } else {
-        current=location[1];
         direction[1]= LEFT;
         wall_follow2(LEFT,BACKWARD,MID,current);
-        location[2]= location[1]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        location[2]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     }
  
     leftTurn();
+    slightleft();
     overBump(RIGS);
+    pc.printf("overbump rigs\r\n");
 }
  
 void rig_section(float* location, float &current, int* direction, int rig)