Revision 16:0888096bff60, committed 2014-04-03

Comitter:

Fairy_Paolina

Date:

Thu Apr 03 15:25:17 2014 +0000

Parent:

15:a467af795e57

Commit message:

asd;

Changed in this revision

--- a/PololuQik2.lib	Tue Apr 01 15:42:03 2014 +0000
+++ b/PololuQik2.lib	Thu Apr 03 15:25:17 2014 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/Fairy_Paolina/code/PololuQik2/#df9964aaa00d
+http://mbed.org/users/Fairy_Paolina/code/PololuQik2/#8d650b072fa8

--- a/main.cpp	Tue Apr 01 15:42:03 2014 +0000
+++ b/main.cpp	Thu Apr 03 15:25:17 2014 +0000
@@ -59,6 +59,9 @@
 void slightMove(int direction, float pulses);
 void us_distance(void);
 void tools_section(float* location, float &current);
+void to_tools_section1(float* location, float &current);
+void to_tools_section2(float* location, float &current);
+void from_tools_section(float* location, float &current);
 void mid_section(float* location, float &current, int* direction);
 void mid_section2(float* location, float &current, int* direction);
 void rig_section(float* location, float &current, int* direction, int rig);
@@ -85,7 +88,21 @@
 
     pc.printf("START\r\n");
     //Go to tools
-    tools_section(location, current);
+    //tools_section(location, current);
+    
+    //If first rig is on fire
+    to_tools_section1(location,current);
+    
+    //If the second rig is on fire
+    /*
+    to_tools_section2(location, current);
+    slightMove(FORWARD,3100);
+    current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+    */
+    
+    wait(2);
+    from_tools_section(location,current);
+    
     mid_section(location, current, direction);
     mid_section2(location, current, direction);
     rig_section(location, current, direction, 3);
@@ -198,10 +215,7 @@
     }
     
     //STOP
-    motors.setMotor0Speed(dir*-0.3*127); //right
-    motors.setMotor1Speed(dir*-0.3*127); //left
-    wait_ms(10);
-    motors.stopBothMotors();
+    motors.stopBothMotors(127);
 }
 
 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
@@ -209,7 +223,7 @@
 void wall_follow2(int side, int direction, int section, float location, int rig)
 {
     int dir=1, limit=86, lowlim=5;
-    float set=6, loc=0, Rigloc=0;
+    float set=7, loc=0, Rigloc=0;
     bool SeeWaveGap = false;
 
     if(rig == 1) Rigloc= 16;
@@ -263,10 +277,7 @@
             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();
+                    motors.stopBothMotors(127);
                     break;
                 }
             }
@@ -284,10 +295,7 @@
                     SeeWaveGap=true;
                 }else {
                     //STOP
-                    motors.setMotor0Speed(dir*-0.25*127); //right
-                    motors.setMotor1Speed(dir*-0.25*127); //left
-                    wait_ms(5);
-                    motors.stopBothMotors();
+                    motors.stopBothMotors(127);
 
                     pc.printf("wavegap\r\n");
                     // AT WAVE OPENING!!!!
@@ -324,10 +332,7 @@
     }
 
     //STOP
-    motors.setMotor0Speed(dir*-0.3*127); //right
-    motors.setMotor1Speed(dir*-0.3*127); //left
-    wait_ms(5);
-    motors.stopBothMotors();
+    motors.stopBothMotors(127);
 }
 
 
@@ -343,7 +348,7 @@
         motors.setMotor0Speed(-1.2*MAX_SPEED); //right
         motors.setMotor1Speed(0.4*MAX_SPEED); //left
         while(rightEncoder.getPulses()>-1000);
-        motors.stopBothMotors();
+        motors.stopBothMotors(0);
 
         //go backwards toward wall
         leftEncoder.reset();
@@ -351,7 +356,7 @@
         motors.setMotor0Speed(-0.25*127); //right
         motors.setMotor1Speed(-0.25*127); //left
         while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
-        motors.stopBothMotors();
+        motors.stopBothMotors(0);
 
         // turn left towards wall
         leftEncoder.reset();
@@ -360,35 +365,37 @@
         motors.setMotor1Speed(-MAX_SPEED); //left
         while(rightEncoder.getPulses() < 20 || abs(leftEncoder.getPulses()) < 20);
 
-        motors.stopBothMotors();
+        motors.stopBothMotors(0);
 
         // turning left
         motors.setMotor0Speed(0.9*MAX_SPEED); //right
         motors.setMotor1Speed(-0.9*MAX_SPEED); //left
 
-    } else if( section == RIGS) {
+    } else if(section == RIGS) {
         // check distance to wall
         rangeFinderRight.startMeas();
         wait_ms(20);
         rangeFinderRight.getMeas(range);
 
-        if(range < 4 || range > 20) return;
+        if(range < 3 || range > 20) return;
 
         // turn 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())<1000);
-        motors.stopBothMotors();
+        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()) < 150 || abs(rightEncoder.getPulses()) < 150);
-        motors.stopBothMotors();
+        while(abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200);
+        motors.stopBothMotors(0);
+        wait(2);
 
         // turn left towards wall
         leftEncoder.reset();
@@ -397,12 +404,13 @@
         motors.setMotor1Speed(MAX_SPEED); //left
         while(abs(rightEncoder.getPulses()) < 20 || abs(leftEncoder.getPulses()) < 20);
 
-        motors.stopBothMotors();
+        motors.stopBothMotors(0);
+        wait(2);
 
         // turning left
         motors.setMotor0Speed(-0.9*MAX_SPEED); //right
         motors.setMotor1Speed(0.9*MAX_SPEED); //left
-    } else {
+    } else {// MID
         pc.printf("in mid section align\r\n");
         // turn right towards wall
         rightTurn();
@@ -429,7 +437,7 @@
             usValue = range;
         }
     }
-    motors.stopBothMotors();
+    motors.stopBothMotors(0);
 }
 
 void rightTurn(void)
@@ -439,8 +447,8 @@
     rightEncoder.reset();
     motors.setMotor0Speed(-0.5*127);//right
     motors.setMotor1Speed(0.5*127);//left
-    while(abs(leftEncoder.getPulses())<950 || abs(rightEncoder.getPulses())<950);
-    motors.stopBothMotors();
+    while(abs(leftEncoder.getPulses())<1050 || abs(rightEncoder.getPulses())<1050);
+    motors.stopBothMotors(127);
 }
 
 void leftTurn(void)
@@ -450,8 +458,8 @@
     rightEncoder.reset();
     motors.setMotor0Speed(0.5*127);// right
     motors.setMotor1Speed(-0.5*127);// left
-    while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100);
-    motors.stopBothMotors();
+    while(abs(leftEncoder.getPulses())<1150 || rightEncoder.getPulses()<1150);
+    motors.stopBothMotors(127);
 }
 
 void slightleft(void)
@@ -462,7 +470,7 @@
     motors.setMotor0Speed(0.5*127);// right
     motors.setMotor1Speed(-0.5*127);// left
     while(abs(leftEncoder.getPulses())<90 || rightEncoder.getPulses()<90);
-    motors.stopBothMotors();
+    motors.stopBothMotors(127);
 }
 
 void slightright(void)
@@ -472,8 +480,8 @@
     rightEncoder.reset();
     motors.setMotor0Speed(-0.4*127);// right
     motors.setMotor1Speed(0.4*127);// left
-    while(abs(leftEncoder.getPulses())<90 || abs(rightEncoder.getPulses())<90);
-    motors.stopBothMotors();
+    while(abs(leftEncoder.getPulses())<50 || abs(rightEncoder.getPulses())<50);
+    motors.stopBothMotors(127);
 }
 
 void slightMove(int direction, float pulses)
@@ -487,11 +495,8 @@
     motors.setMotor0Speed(dir*0.25*127); //right
     motors.setMotor1Speed(dir*0.25*127); //left
     while(abs(leftEncoder.getPulses()) < pulses || abs(rightEncoder.getPulses()) < pulses);
-
-    motors.setMotor0Speed(dir*-0.25*127); //right
-    motors.setMotor1Speed(dir*-0.25*127); //left
-    wait_ms(10);
-    motors.stopBothMotors();
+    
+    motors.stopBothMotors(127);
 }
 
 void UntilWall(int dir)
@@ -512,10 +517,7 @@
         rangeFinderRight.getMeas(range);
     }
 
-    motors.setMotor0Speed(dir*-0.2*127); //right
-    motors.setMotor1Speed(dir*-0.2*127); //left
-    wait_ms(5);
-    motors.stopBothMotors();
+    motors.stopBothMotors(127);
 }
 
 void overBump(int section)
@@ -529,55 +531,29 @@
     motors.setMotor0Speed(-0.25*127); //right
     motors.setMotor1Speed(-0.25*127); //left
     while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50);
-    motors.stopBothMotors();
+    motors.stopBothMotors(127);
 
     pc.printf("slight backwards\r\n");
     wait_ms(200);
-
+    
+    // Over bump
     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() >15 ) {
-        /*preLeft=leftEncoder.getPulses();
+        /*
+        preLeft=leftEncoder.getPulses();
         preRight=rightEncoder.getPulses();
         wait_ms(200);
-        if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;*/
+        if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
+        */
     }
 
     pc.printf("forward \r\n");
     wait_ms(200);
-    /*
-       motors.stopBothMotors();
-       motors.begin();
 
-       leftEncoder.reset();
-       rightEncoder.reset();
-       motors.setMotor0Speed(0.3*127); //right
-       motors.setMotor1Speed(0.3*127); //left
-
-       while(!out) {
-           preLeft=leftEncoder.getPulses();
-           preRight=rightEncoder.getPulses();
-
-           rangeFinderLeft.startMeas();
-           rangeFinderRight.startMeas();
-           wait_ms(20);
-           rangeFinderLeft.getMeas(range);
-           rangeFinderRight.getMeas(range2);
-           if(range < 10 || range2 < 10) out=1;
-
-           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();
+    motors.stopBothMotors(0);
     motors.begin();
 
     preLeft=preRight=5000 ;
@@ -587,7 +563,7 @@
     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)) {
+        while(IR.getDistance() > 10 && (abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
 
             if(IR.getDistance() > 38) break;
 
@@ -597,7 +573,7 @@
         }
     } else if(section == MID || section == MID2) {
         if(section == MID2) while(IR.getDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400));
-        while(IR.getDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400) && (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;
 
@@ -606,9 +582,10 @@
             wait_ms(200);
         }
 
-    } else {
-        while(abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100);
-
+    } else {// RIGS
+        while(abs(leftEncoder.getPulses()) < 220 || abs(rightEncoder.getPulses()) < 220);
+        
+        // go backwards to line up with bump
         leftEncoder.reset();
         rightEncoder.reset();
 
@@ -620,32 +597,72 @@
             wait_ms(200);
             if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
         }
-
-        leftEncoder.reset();
-        rightEncoder.reset();
-
-        motors.setMotor0Speed(0.25*127); //right
-        motors.setMotor1Speed(0.25*127); //left
-        while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10));
-
-        motors.stopBothMotors();
+        motors.stopBothMotors(127);
 
         return;
     }
 
-    leftEncoder.reset();
-    rightEncoder.reset();
-
-    motors.setMotor0Speed(-.25*127); //right
-    motors.setMotor1Speed(-.25*127); //left
-    while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10));
-
-    motors.stopBothMotors();
+    motors.stopBothMotors(127);
     wait_ms(20);
     motors.begin();
 
 }
-
+void to_tools_section1(float* location, float &current)
+{
+    slightMove(FORWARD,6600);
+    current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+ 
+}
+ 
+void to_tools_section2(float* location, float &current)
+{
+    slightMove(FORWARD,3200);
+    current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+ 
+}
+ 
+void from_tools_section(float* location, float &current)
+{
+ 
+    alignWithWall(TOOLS);
+    pc.printf("align\r\n");
+    wait_ms(100);
+ 
+    //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);
+    rangeFinderLeft.getMeas(range);
+ 
+    if(range < 20) {
+        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];
+        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
+        motors.stopBothMotors(127);
+ 
+        wait_ms(100);
+        leftTurn();
+        overBump(TOOLS);
+    } else {
+        pc.printf("else greater than 20\r\n");
+        location[0]= current;
+        leftTurn();
+        overBump(TOOLS);
+    }
+ 
+    pc.printf("First Wavegap = %f\r\n",location[0]);
+ 
+}
 void tools_section(float* location, float &current)
 {
     wall_follow(LEFT,FORWARD, TOOLS);
@@ -686,12 +703,8 @@
         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()) < 10 || abs(rightEncoder.getPulses())< 10);
-        motors.stopBothMotors();
+    
+        motors.stopBothMotors(127);
 
         wait_ms(100);
         leftTurn();
@@ -784,14 +797,14 @@
     if(range > 20 ) {
         direction[1]= RIGHT;
         location[2]= current;
-        slightMove(FORWARD,75);
-        //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        slightMove(FORWARD,100);
+        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(FORWARD,500);
+        //slightMove(FORWARD,100);
     }
 
     leftTurn();
@@ -807,9 +820,13 @@
     else if(rig == 2) loc= 45;
     else loc = 75;
 
+    // Slight forward for turn
+    slightMove(FORWARD,100);
+    wait_ms(100);
     rightTurn();
     slightright();
-
+    wait(5);
+    
     if(current > loc) {
         pc.printf("RIG section %f\r\n",current);
         wall_follow2(RIGHT, BACKWARD, RIGS, current, rig);
@@ -819,6 +836,8 @@
         wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
         current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     }
+    
+    alignWithWall(RIGS);
 }
 
 void tools_section_return(float* location, float &current)
@@ -827,7 +846,7 @@
         leftTurn();
         wall_follow2(LEFT, BACKWARD, RETURN, location[0], 0);
     }
-    motors.stopBothMotors();
+    motors.stopBothMotors(0);
 
 }
 
@@ -868,7 +887,6 @@
 
 void rig_section_return(float* location, float &current, int* direction)
 {
-    alignWithWall(RIGS);
     if(location[2] > current) {
         wall_follow2(RIGHT, FORWARD, MID, current,0);
         current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);