Diff: main.cpp

Revision:

23:f8e806d1ffcc

Parent:

22:79c5871543b5

Child:

24:3369d51f6cbd

--- a/main.cpp	Fri Apr 11 21:56:18 2014 +0000
+++ b/main.cpp	Sat Apr 12 20:27:00 2014 +0000
@@ -303,7 +303,6 @@
                     **************************************************/
                 case START :
                     myled1 = 1;
-                    //state = GOTO_TOOLS1;
                     state = OILRIG1_POS;
                     break;
  
@@ -376,7 +375,6 @@
                     wait(3);                                //wait for servos to settle
                     
                     to_tools_section1(location, current);
-                    //state  = NAVIGATE_WAVES_ROW1;
                     state = IDENTIFY_TOOLS;
                     break;
  
@@ -637,12 +635,12 @@
                     break;
  
                 case NAVIGATE_WAVES_ROW3:
-                    shape_detected = 1;
-                    if(shape_detected == 1) {
+                    //shape_detected = 1;
+                    if(tool_needed == 1) {
                         rig_section(location, current, direction, 1);
                         //state = NAVIGATE_TO_SQUARE_RIG;
                         state = RETURN_TO_START;
-                    } else if(shape_detected == 2) {
+                    } else if(tool_needed == 2) {
                         rig_section(location, current, direction, 2);
                         //state = NAVIGATE_TO_TRIANGLE_RIG;
                         state = RETURN_TO_START;
@@ -714,13 +712,10 @@
                     *
                     **************************************************/
                 case RETURN_TO_START:
-                    wait(3);
+                wait(3);
                     rig_section_return(location, current, direction);
-                    wait(3);
                     mid_section2_return(location, current, direction);
-                    wait(3);
                     mid_section_return(location, current, direction);
-                    wait(3);
                     tools_section_return(location,current);
                     while(1);
                     state = END;
@@ -970,7 +965,7 @@
 void wall_follow2(int side, int direction, int section, float location, int rig)
 {
     int dir=1, limit=80, lowlim=4;
-    float set=9, loc=0, Rigloc=0;
+    float set=9, loc=0, Rigloc=0, location_cal=0;
     bool SeeWaveGap = false;
  
     if(rig == 1) Rigloc= 16;
@@ -980,10 +975,9 @@
     pid1.reset();
  
     if(section == TOOLS) {
-        set= 9;
-        limit = 86;
-    } else if(section == RIGS) set = 9;
-    else if(section == RETURN) lowlim=4;
+        limit = 100;
+        lowlim=10;
+    }else if(section == RIGS) set = 6;
     else if(section == MID2) limit =85;
  
     if(direction == BACKWARD) {
@@ -1000,11 +994,15 @@
  
     //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 >= lowlim)) {
+    if((section == RIGS || section == RETURN) && side == LEFT)location_cal= -1*dir*loc + location;
+    else location_cal= dir*loc + location;
  
+    while((location_cal <= limit) && (location_cal >= lowlim)) {
+             
         loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-        //pc.printf("loc %f \r\n", loc);
+        
+        if((section == RIGS || section == RETURN) && side == LEFT)location_cal= -1*dir*loc + location;
+        else location_cal= dir*loc + location;
  
         pid1.setInputLimits(0.0, set);
         pid1.setOutputLimits( -MAX_SPEED1, MAX_SPEED1);
@@ -1021,12 +1019,18 @@
         }
  
         if(section == RIGS) {
-            rangeFinderLeft.startMeas();
-            wait_ms(20);
-            rangeFinderLeft.getMeas(range2);
+            if(side == LEFT){
+                rangeFinderRight.startMeas();
+                wait_ms(20);
+                rangeFinderRight.getMeas(range2);
+            }else{
+                rangeFinderLeft.startMeas();
+                wait_ms(20);
+                rangeFinderLeft.getMeas(range2);
+            }
  
             if(range2< 15) {
-                if( abs(dir*loc + location - Rigloc) < 10) {
+                if( abs(location_cal - Rigloc) < 10) {
                     //STOP
                     motors.stopBothMotors(127);
                     break;
@@ -1038,7 +1042,7 @@
         //pc.printf("wall follow 2 range %f\r\n",range);
         //pc.printf("loc+location = %f\r\n", loc+location);
         if(range > 15 ) {
-            if(section == RIGS || section == RETURN) {
+            if(section == RIGS || section == TOOLS) {
                 motors.setMotor0Speed(dir*0.25*127); //right
                 motors.setMotor1Speed(dir*0.25*127); //left
             } else {
@@ -1141,78 +1145,77 @@
         //motors.setMotor1Speed(-0.9*MAX_SPEED); //left
  
     } else if(section == RIGS) {
-        // check distance to wall
-        rangeFinderRight.startMeas();
-        wait_ms(20);
-        rangeFinderRight.getMeas(range);
  
-        if(range < 3) return;
- 
-        // turn at an angle
+        // turn left at an angle
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor1Speed(-1.2*MAX_SPEED); //left
         motors.setMotor0Speed(0.4*MAX_SPEED); //right
         while(abs(leftEncoder.getPulses())<500);
         motors.stopBothMotors(0);
-        wait(2);
- 
-        //go backwards toward wall
-        leftEncoder.reset();
-        rightEncoder.reset();
-        motors.setMotor0Speed(-0.25*127); //right
-        motors.setMotor1Speed(-0.25*127); //left
-        while(abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200);
-        motors.stopBothMotors(0);
-        wait(2);
+        wait_ms(200);
  
-        // turn right towards wall
-        leftEncoder.reset();
-        rightEncoder.reset();
-        motors.setMotor0Speed(-MAX_SPEED); //right
-        motors.setMotor1Speed(MAX_SPEED); //left
-        while(abs(rightEncoder.getPulses()) < 50 || abs(leftEncoder.getPulses()) < 50);
- 
-        motors.stopBothMotors(127);
-        /*        wait(2);
- 
-                // turning left
-                motors.setMotor0Speed(-0.9*MAX_SPEED); //right
-                motors.setMotor1Speed(0.9*MAX_SPEED); //left
-        */
-    } else if(section == MID2) {
-        leftEncoder.reset();
-        rightEncoder.reset();
-        motors.setMotor0Speed(-1.2*MAX_SPEED); //right
-        motors.setMotor1Speed(0.4*MAX_SPEED); //left
-        while(rightEncoder.getPulses()>-1000);
-        motors.stopBothMotors(0);
- 
-        //go backwards toward wall
+        //go backwards away form wall
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(-0.25*127); //right
         motors.setMotor1Speed(-0.25*127); //left
         while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150);
+        motors.stopBothMotors(127);
+        wait(2);
  
-        // turn left towards wall
+        // turn right away from wall
         leftEncoder.reset();
         rightEncoder.reset();
-        motors.setMotor0Speed(0.4*127); //right
-        motors.setMotor1Speed(-0.4*127); //left
-        while(abs(rightEncoder.getPulses()) < 65 || abs(leftEncoder.getPulses()) < 65);
+        motors.setMotor0Speed(-MAX_SPEED); //right
+        motors.setMotor1Speed(MAX_SPEED); //left
+        while(abs(rightEncoder.getPulses()) < 25 || abs(leftEncoder.getPulses()) < 25 );
+ 
         motors.stopBothMotors(127);
- 
-        slightMove(FORWARD,100);
+
+    } else if(section == MID2) {
+
+        // check distance to wall
+        rangeFinderLeft.startMeas();
+        wait_ms(20);
+        rangeFinderLeft.getMeas(range);
+
+        if(range > 4) {
+
+            leftEncoder.reset();
+            rightEncoder.reset();
+            motors.setMotor0Speed(-1.2*MAX_SPEED); //right
+            motors.setMotor1Speed(0.4*MAX_SPEED); //left
+            while(rightEncoder.getPulses()>-1000);
+            motors.stopBothMotors(0);
+
+            //go backwards toward wall
+            leftEncoder.reset();
+            rightEncoder.reset();
+            motors.setMotor0Speed(-0.25*127); //right
+            motors.setMotor1Speed(-0.25*127); //left
+            while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150);
+
+            // turn left towards wall
+            leftEncoder.reset();
+            rightEncoder.reset();
+            motors.setMotor0Speed(0.4*127); //right
+            motors.setMotor1Speed(-0.4*127); //left
+            while(abs(rightEncoder.getPulses()) < 65 || abs(leftEncoder.getPulses()) < 65);
+            motors.stopBothMotors(127);
+
+            slightMove(FORWARD,100);
+        }
         return;
  
-    } else { // MID
+    }
+    else{ // MID
         //pc.printf("in mid section align\r\n");
         // turn right towards wall
         rightTurn();
         // turning left towards wall
-        motors.setMotor0Speed(0.9*MAX_SPEED); //right
-        motors.setMotor1Speed(-0.9*MAX_SPEED); //left
+        //motors.setMotor0Speed(0.9*MAX_SPEED); //right
+       //motors.setMotor1Speed(-0.9*MAX_SPEED); //left
  
     }
  
@@ -1337,8 +1340,8 @@
     // Over bump
     leftEncoder.reset();
     rightEncoder.reset();
-    motors.setMotor0Speed(0.3*127); //right
-    motors.setMotor1Speed(0.3*127); //left
+    motors.setMotor0Speed(0.33*127); //right
+    motors.setMotor1Speed(0.33*127); //left
     while((abs(leftEncoder.getPulses()) < 900 || abs(rightEncoder.getPulses()) < 900) /*&& preLeft!=0*/ && IR.getIRDistance() >15 );
  
  
@@ -1366,18 +1369,18 @@
         }
     } else if(section == MID || section == MID2) {
         if(section == MID2) {
-            motors.setMotor0Speed(.3*127); //right
-            motors.setMotor1Speed(.3*127); //left
-            while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300));
+            motors.setMotor0Speed(.33*127); //right
+            motors.setMotor1Speed(.33*127); //left
+            while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400));
         }
  
-        while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && (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;
+            //if(IR.getIRDistance() > 38) break;
  
-            preLeft=leftEncoder.getPulses();
-            preRight=rightEncoder.getPulses();
-            wait_ms(200);
+            //preLeft=leftEncoder.getPulses();
+            //preRight=rightEncoder.getPulses();
+            //wait_ms(200);
         }
  
     } else {// RIGS
@@ -1435,7 +1438,7 @@
  
     //wall_follow2(LEFT,FORWARD,MID, current);
  
-    slightMove(BACKWARD,400);
+    slightMove(BACKWARD,500);
     current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
  
     rangeFinderLeft.startMeas();
@@ -1544,11 +1547,12 @@
     //current-=4;
     //}
     rightTurn();
+    //right turn more
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(-0.4*127);// right
     motors.setMotor1Speed(0.4*127);// left
-    while(abs(leftEncoder.getPulses())<100 || abs(rightEncoder.getPulses())<100);
+    while(abs(leftEncoder.getPulses())<25 || abs(rightEncoder.getPulses())<25);
     motors.stopBothMotors(127);
     //pc.printf("mid section current = %f\r\n",current);
     wall_follow2(LEFT,FORWARD,MID, current,0);
@@ -1561,7 +1565,7 @@
     rangeFinderLeft.getMeas(range);
  
     if(range > 20 ) {
-        wait(3);
+        wait(1);
         direction[0]= RIGHT;
         location[1]= current;
         wait_ms(300);
@@ -1577,7 +1581,7 @@
             slightMove(FORWARD, 75);
             current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
         }
-        //slightMove(BACKWARD,100);
+        else slightMove(BACKWARD,75);
  
     }
  
@@ -1589,7 +1593,7 @@
     rightEncoder.reset();
     motors.setMotor0Speed(0.5*127);// right
     motors.setMotor1Speed(-0.5*127);// left
-    while(abs(leftEncoder.getPulses())<1045 || rightEncoder.getPulses()<1045);
+    while(abs(leftEncoder.getPulses())<110 || rightEncoder.getPulses()<1100);
     motors.stopBothMotors(127);
  
     wait_ms(100);
@@ -1633,7 +1637,7 @@
     if(range > 20 ) {
         direction[1]= RIGHT;
         location[2]= current;
-        slightMove(FORWARD,100);
+        slightMove(FORWARD,150);
         current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     } else {
         direction[1]= LEFT;
@@ -1649,7 +1653,7 @@
     rightEncoder.reset();
     motors.setMotor0Speed(0.5*127);// right
     motors.setMotor1Speed(-0.5*127);// left
-    while(abs(leftEncoder.getPulses())<1050 || rightEncoder.getPulses()<1050);
+    while(abs(leftEncoder.getPulses())<1115 || rightEncoder.getPulses()<1115);
     motors.stopBothMotors(127);
  
     overBump(RIGS);
@@ -1666,26 +1670,25 @@
  
     // Slight forward for turn
     slightMove(FORWARD,150);
-    current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     wait_ms(100);
-    rightTurn();
+    leftTurn();
     //slightright();
  
  
     if(current > loc) {
         //pc.printf("RIG section %f\r\n",current);
-        wall_follow2(RIGHT, BACKWARD, RIGS, current, rig);
+        wall_follow2(LEFT, FORWARD, RIGS, current, rig);
         current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     } else {
         //pc.printf("RIG section %f\r\n",current);
-        wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
+        wall_follow2(LEFT, BACKWARD, RIGS, current, rig);
         current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     }
-    wait(2);
+    wait(1);
     alignWithWall(MID2);
-    current-=4;
-    wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
-    current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+    current+=4;
+    wall_follow2(LEFT, FORWARD, RIGS, current, rig);
+    current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     slightMove(FORWARD, 75);
     current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
 }
@@ -1694,25 +1697,28 @@
 {
     if(location[0] > 16) {
         leftTurn();
-        wall_follow2(LEFT, BACKWARD, RETURN, location[0], 0);
+        wall_follow2(LEFT, BACKWARD, TOOLS, location[0], 0);
     }
-    motors.stopBothMotors(0);
+    motors.stopBothMotors(127);
  
 }
  
 void mid_section_return(float* location, float &current, int* direction)
 {
     if(direction[0] == RIGHT) {
-        leftTurn();
-        alignWithWall(MID);
-        wall_follow2(LEFT, BACKWARD, MID, current,0);
-        current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
         rightTurn();
-    } else if(direction[0] == LEFT) {
+        alignWithWall(MID);
+        wall_follow2(LEFT, FORWARD, MID, current,0);
+        current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        slightMove(FORWARD, 50);
         leftTurn();
-        wall_follow2(RIGHT, FORWARD, MID, current,0);
+    } else if(direction[0] == LEFT) {
+        rightTurn();
+        wall_follow2(RIGHT, BACKWARD, MID, current,0);
         current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-        rightTurn();
+        wait_ms(200);
+        slightMove(BACKWARD, 100);
+        leftTurn();
     }
     //ELSE and GO FORWARD
     overBump(RIGS);
@@ -1721,15 +1727,15 @@
 void mid_section2_return(float* location, float &current, int* direction)
 {
     if(direction[1] == RIGHT) {
+        rightTurn();
+        wall_follow2(LEFT, FORWARD, RETURN, current,0);
+        current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
         leftTurn();
-        wall_follow2(LEFT, BACKWARD, MID, current,0);
-        current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-        rightTurn();
     } else if(direction[1] == LEFT) {
+        rightTurn();
+        wall_follow2(LEFT, BACKWARD, RETURN, current,0);
+        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
         leftTurn();
-        wall_follow2(RIGHT, FORWARD, MID, current,0);
-        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-        rightTurn();
     }
     //ELSE and GO FORWARD
     overBump(MID);
@@ -1737,14 +1743,27 @@
  
 void rig_section_return(float* location, float &current, int* direction)
 {
+    alignWithWall(RIGS);
+    
     if(location[2] > current) {
-        wall_follow2(RIGHT, FORWARD, MID, current,0);
+        wall_follow2(LEFT, BACKWARD, RETURN, current,0);
         current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        wait_ms(500);
+        //slightMove(FORWARD,50);
     } else {
-        wall_follow2(RIGHT, BACKWARD, MID, current,0);
+        wall_follow2(LEFT, FORWARD, RETURN, current,0);
         current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     }
-    rightTurn();
+    
+    // LEFT TURN
+    motors.begin();
+    leftEncoder.reset();
+    rightEncoder.reset();
+    motors.setMotor0Speed(0.5*127);// right
+    motors.setMotor1Speed(-0.5*127);// left
+    while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<110);
+    
+    motors.stopBothMotors(127);
     overBump(MID2);
 }