Diff: main.cpp

Revision:

6:f5c26372b2d0

Parent:

5:70ccef3734ae

Child:

7:78745a518957

--- a/main.cpp	Thu Mar 20 17:47:31 2014 +0000
+++ b/main.cpp	Fri Mar 21 21:26:50 2014 +0000
@@ -23,7 +23,9 @@
 #define BACKWARD       (0) 
 #define TOOLS          (0)
 #define MID            (1)
-#define RIGS           (2)  
+#define FIRST_WAVE     (0)
+#define FAR            (1)
+
  
 float range, pid_return;
 void errFunction(void);
@@ -44,14 +46,17 @@
 //Functions
  
 float wall_follow(int side, int direction, int section);
-void wall_follow2(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);
 void rightTurn(void);
 void us_distance(void);
 void tools_section(float* location, float &current);
-void overBump(void);
-void align(float speed);
+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);
+void overBump(int wave);
+void alignWithWall(int section);
  
 //Variables
  
@@ -64,75 +69,11 @@
     bt.baud(115200);
     motors.begin();
     
-    
-    //leftEncoder.reset();
-    //rightEncoder.reset();
-    //motors.setMotor0Speed(MAX_SPEED); //right
-    //motors.setMotor1Speed(MAX_SPEED); //left
-    
-    //while((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2 < 3)
- 
-    
-    
     //Go to tools
     tools_section(location, current);
-   
-    //////////////////////////////// without predefined wavegaps//////////////////////////////////////////////
- /*   current=0;
-    if(location[0]< 75){
-        rightTurn();
-        current=wall_follow(LEFT,FORWARD,MID);
-        if(current != 0) direction[0]= RIGHT;
-        else{
-            direction[0]= LEFT;
-            wall_follow2(LEFT,BACKWARD,MID);
-            location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-            
-            leftEncoder.reset();
-            rightEncoder.reset();
-            motors.setMotor0Speed(-MAX_SPEED); //right
-            motors.setMotor1Speed(-MAX_SPEED); //left
-            while(abs(leftEncoder.getPulses())<75 || abs(rightEncoder.getPulses())<75);
-            motors.stopBothMotors();
-        }
-        leftTurn();
-    }
-    else{  
-        leftTurn();
-        direction[0]= LEFT;
-        wall_follow2(RIGHT,FORWARD,MID);
-        location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;  
-        rightTurn();
-    }
-        
-    overBump();
-    
-    */
-    
-    
-    // left or right
-    
-    
-    
-    
-    
-    
-     
-    
-    
-    //leftTurn();
-    //wall_follow2(RIGHT);
-   // rightTurn();
-    
+    mid_section(location, current, direction);
+    mid_section2(location, current, direction);
  
- 
-    bt.printf("LOCATION %f\n\r",location);
- 
-    //overBump();
-    //motors.stopBothMotors();
-   //leftTurn();
-   //wait(1);
-   //rightTurn();
    
  
 }
@@ -154,7 +95,7 @@
  
 float wall_follow(int side, int direction, int section)
 {
-    float location, wavegap=0, set=5;
+    float location, wavegap=0, set=4;
     int dir=1;
     
     pid1.reset();
@@ -167,7 +108,7 @@
     
     location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
     
-    while(location< 75) {
+    while(location< 78) {
         location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
         
         pid1.setInputLimits(0, set);
@@ -189,32 +130,36 @@
             wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
             bt.printf("wavegap %f\r\n",wavegap);
             // AT WAVE OPENING!!!!
+            motors.setMotor1Speed(dir*0.3*127);//left
+            motors.setMotor0Speed(dir*0.3*127);//right
         }
+        else{
         
-        pid1.setProcessValue(range);
-        pid_return = pid1.compute();
+            pid1.setProcessValue(range);
+            pid_return = pid1.compute();
  
-        if(pid_return > 0) {
-            if(side){
-                motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
+            if(pid_return > 0) {
+                if(side){
+                    motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
+                    motors.setMotor1Speed(dir*MAX_SPEED);//left
+                }
+                else{
+                    motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
+                    motors.setMotor0Speed(dir*MAX_SPEED);//right
+                }
+            }else if(pid_return < 0) {
+                if(side){
+                    motors.setMotor0Speed(dir*MAX_SPEED);//right
+                    motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
+                }
+                else{
+                    motors.setMotor1Speed(dir*MAX_SPEED);//left
+                    motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
+                }
+            }else {
+                motors.setMotor0Speed(dir*MAX_SPEED);//right
                 motors.setMotor1Speed(dir*MAX_SPEED);//left
             }
-            else{
-                motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
-                motors.setMotor0Speed(dir*MAX_SPEED);//right
-            }
-        }else if(pid_return < 0) {
-            if(side){
-                motors.setMotor0Speed(dir*MAX_SPEED);//right
-                motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
-            }
-            else{
-                motors.setMotor1Speed(dir*MAX_SPEED);//left
-                motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
-            }
-        }else {
-            motors.setMotor0Speed(dir*MAX_SPEED);//right
-            motors.setMotor1Speed(dir*MAX_SPEED);//left
         }
     }
     return wavegap;
@@ -222,21 +167,22 @@
  
 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
  
-void wall_follow2(int side, int direction, int section)
+void wall_follow2(int side, int direction, int section, float location)
 {
     int SeeWaveGap = false, dir=1;
-    float set=5;
+    float set=4, loc;
     
     pid1.reset();
     
     if(direction == BACKWARD) dir=-1;
-    if(section == TOOLS)set= 6.5;
+    if(section == TOOLS)set= 5;
     
     leftEncoder.reset();
     rightEncoder.reset();
  
-    while(1) {
- 
+    while(loc - location ) {
+        location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+        
         pid1.setInputLimits(0.0, set);
         pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
         pid1.setSetPoint(set);
@@ -292,76 +238,68 @@
             motors.setMotor1Speed(dir*MAX_SPEED);
         }
     }
+    motors.stopBothMotors();
 }
  
  
-/* MODIFIED WALL_FOLLOW FOR NAVIGATION WITH WAVE OPENINGS PASSED IN */
-/* MEANT FOR RETURNING FROM OIL RIGS */
- 
-void wall_follow3(int &currentLocation, int &WaveOpening)
-{
-    while(1) {
- 
- 
-        pid1.setInputLimits(0, 5);
-        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
-        pid1.setSetPoint(5.0);
- 
+void alignWithWall(int section){
+    float usValue = 0;
+    
+    if(section == TOOLS){
+        // turn at an angle
+        leftEncoder.reset();
+        rightEncoder.reset(); 
+        motors.setMotor0Speed(-MAX_SPEED); //right
+        motors.setMotor1Speed(0.4*MAX_SPEED); //left
+        while(rightEncoder.getPulses()>-1000);
+        motors.stopBothMotors();
+        
+        //go backwards toward wall
+        leftEncoder.reset();
+        rightEncoder.reset(); 
+        motors.setMotor0Speed(-MAX_SPEED); //right
+        motors.setMotor1Speed(-MAX_SPEED); //left
+        while(abs(leftEncoder.getPulses()) < 350 || abs(rightEncoder.getPulses()) < 350);
+        
+        motors.stopBothMotors();
+    
+        // turn left towards wall 
+        leftEncoder.reset();
+        rightEncoder.reset();
+        motors.setMotor0Speed(MAX_SPEED); //right
+        motors.setMotor1Speed(-MAX_SPEED); //left
+        while(rightEncoder.getPulses() < 100);
+        
+        motors.stopBothMotors();   
+    }
+    else{
+        // turn right towards wall 
+        leftEncoder.reset();
+        rightEncoder.reset();
+        motors.setMotor0Speed(-MAX_SPEED); //right
+        motors.setMotor1Speed(MAX_SPEED); //left
+        while(abs(rightEncoder.getPulses()) < 300);
+        
+        motors.stopBothMotors();
+    }
+    
+    // align using the distance of the wall   
+    motors.setMotor0Speed(0.7*MAX_SPEED); //right
+    motors.setMotor1Speed(-0.7*MAX_SPEED); //left
+    
+    usValue = 0;
+    while(1){
         rangeFinderLeft.startMeas();
-        wait_ms(100);
-        if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) {
-            //bt.printf("Range = %f\n", range);
-        }
- 
-        /*************CHECK FOR WAVE OPENING*****************/
-        /* If after 100 ms the ultrasonic still sees 20+ cm */
-        /*      then robot is at wave opening               */
- 
- 
-        if(range > 20 ) {
-            currentLocation--;
-        }
- 
-        if( currentLocation == WaveOpening) {
-            // AT WAVE OPENING!!!!
- 
+        wait_ms(20);
+        rangeFinderLeft.getMeas(range);
+        bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
+        if(range > usValue && usValue != 0 && range < 25){
             break;
-        }
- 
- 
-        pid1.setProcessValue(range);
-        pid_return = pid1.compute();
-        bt.printf("Range: %f\n      PID:   %f", range, pid_return);
- 
-        if(pid_return > 0) {
-            motors.setMotor0Speed(MAX_SPEED - pid_return);
-            motors.setMotor1Speed(MAX_SPEED);
-        } else if(pid_return < 0) {
-            motors.setMotor0Speed(MAX_SPEED);
-            motors.setMotor1Speed(MAX_SPEED + pid_return);
         } else {
-            motors.setMotor0Speed(MAX_SPEED);
-            motors.setMotor1Speed(MAX_SPEED);
+            usValue = range;    
         }
     }
-}
- 
-void align(float speed){
-    float left, right;  
-    left=right=speed;
-    
-    if(abs(leftEncoder.getPulses())> rightEncoder.getPulses()){
-        if(speed>0)left-=.01;
-        else left+=.01;
-    }
-    else{
-        if(speed>0) right+=.01;
-        else right-=.01;
-    }
-    motors.setMotor0Speed(right*127);//right
-    motors.setMotor1Speed(left*127);//left
-    pc.printf("speed left %f right %f\r\n\n",left, right);
-    
+    motors.stopBothMotors();
 }
  
 void rightTurn(void)
@@ -370,7 +308,7 @@
     rightEncoder.reset();
     motors.setMotor0Speed(-0.4*127);//right
     motors.setMotor1Speed(0.4*127);//left
-    while(leftEncoder.getPulses()<1030 || rightEncoder.getPulses()>-1030);
+    while(leftEncoder.getPulses()<1000 || rightEncoder.getPulses()>-1000);
     motors.stopBothMotors();
 }
  
@@ -380,105 +318,161 @@
     rightEncoder.reset();
     motors.setMotor0Speed(0.4*127);// right
     motors.setMotor1Speed(-0.4*127);// left
-    while(abs(leftEncoder.getPulses())<1120 || rightEncoder.getPulses()<1120); //align(0.4);
+    while(abs(leftEncoder.getPulses())<1075 || rightEncoder.getPulses()<1075);
     motors.stopBothMotors();
 }
  
-void slightRight(void)
-{
-    leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(-0.4*127);//right
-    motors.setMotor1Speed(0.4*127);//left
-    while(leftEncoder.getPulses()<515 || rightEncoder.getPulses()>-515);
-    motors.stopBothMotors();
-}
-void slightLeft(void)
-{ 
-    leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(0.4*127);// right
-    motors.setMotor1Speed(-0.4*127);// left
-    while(abs(leftEncoder.getPulses())<400 || rightEncoder.getPulses()< 400); //align(0.4);
-    motors.stopBothMotors();
-}
 
-void overBump(void){
+void overBump(int wave){
+    int preLeft=5000, preRight=5000 ;
     
     leftEncoder.reset();
     rightEncoder.reset();
-    motors.setMotor0Speed(MAX_SPEED); //right
-    motors.setMotor1Speed(MAX_SPEED); //left
-    while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <9)//align(MAX_SPEED);
+    motors.setMotor0Speed(0.15*127); //right
+    motors.setMotor1Speed(0.15*127); //left
+    while((abs(leftEncoder.getPulses()) < 700 || abs(rightEncoder.getPulses())< 700) && preLeft!=0){
+        preLeft=leftEncoder.getPulses();
+        preRight=rightEncoder.getPulses();
+        wait_ms(20);
+        if(leftEncoder.getPulses() == preLeft && rightEncoder.getPulses()== preRight) preLeft=preRight=0;
+    } 
     
+    if(wave == FAR){
+        while(leftEncoder.getPulses() != preLeft && rightEncoder.getPulses()!= preRight){
+            preLeft=leftEncoder.getPulses();
+            preRight=rightEncoder.getPulses();
+            wait_ms(20);
+        }
+        
+        motors.stopBothMotors();
+    }
+        
     leftEncoder.reset();
     rightEncoder.reset();
-    motors.setMotor0Speed(MAX_SPEED); //right
-    motors.setMotor1Speed(MAX_SPEED); //left
-    while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <6);
-    
+    motors.setMotor0Speed(0.4*127); //right
+    motors.setMotor1Speed(0.4*127); //left
+    while(abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200);
+
     motors.stopBothMotors();
 }
  
 void tools_section(float* location, float &current){
     
-    location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field
+    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("wavegap %f \t current %f \r\n",location[0],current);
+    bt.printf("current %f \r\n",current);
     
     motors.stopBothMotors();
     
-    slightRight();
-    //backward
+    //Tool aquiring
+    wait(2);
+    // After tool is aquired
+    
+    alignWithWall(TOOLS);
+    current-=8;
+    wait_ms(100);
+    
+    rangeFinderLeft.startMeas();
+    wait_ms(20);
+    rangeFinderLeft.getMeas(range);
+    
+    if(range < 20){
+        wall_follow2(LEFT,BACKWARD,TOOLS, current);
+        location[0]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        current= location[0];
+        
+  /*      // go backwards
+        leftEncoder.reset();
+        rightEncoder.reset();
+        motors.setMotor0Speed(-0.3*127); //right
+        motors.setMotor1Speed(-0.3*127); //left
+        while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150);
+        
+        motors.stopBothMotors();
+        */
+        
+        leftTurn();
+        overBump(FAR);
+    }
+    else{   
+        location[0]= 77;     
+        leftTurn();
+        wait_ms(20);
+        overBump(FIRST_WAVE);
+    } 
+    
+    bt.printf("wavegap = %f\r\n",location[0]);
+}
+
+void mid_section(float* location, float &current, int* direction){
+    
+    motors.begin();
+    rightTurn();
+    alignWithWall(MID);
+ /*   
     leftEncoder.reset();
     rightEncoder.reset();
-    motors.setMotor0Speed(-MAX_SPEED);//right
-    motors.setMotor1Speed(-MAX_SPEED);//left
-    while(abs(leftEncoder.getPulses())<500 || abs(rightEncoder.getPulses())<500);
+    motors.setMotor0Speed(MAX_SPEED); //right
+    motors.setMotor1Speed(MAX_SPEED); //left
+    while(abs(leftEncoder.getPulses())<75 || abs(rightEncoder.getPulses())<75);
+    motors.stopBothMotors();
+    */
+    bt.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){
+        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);
+    }
+    
+    bt.printf("wavegap2 = %f\r\n",location[1]);
+    leftTurn();    
+    overBump(FAR);
+    // 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();
     
-    slightLeft();
+}
+    
+void mid_section2(float* location, float &current, int* direction){
+    
+    motors.begin();
+    rightTurn();
+    alignWithWall(MID);
+    wall_follow2(LEFT,FORWARD,MID, current);
+    current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     
-    /*leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(MAX_SPEED);//right
-    motors.setMotor1Speed(MAX_SPEED);//left
-    while(abs(leftEncoder.getPulses())<100 || abs(rightEncoder.getPulses())<100);*/
+    if(current != 0){
+        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);
+    }
     
+    leftTurn();    
+    overBump(FAR);
+}    
+
+void rig_section(float* location, float &current, int* direction, int rig){
     
     
-    ////////////////////////////////////////// determine shape and pick up tool ///////////////////////////////////////////////////////
-    
-    wall_follow2(LEFT,BACKWARD,TOOLS);
-    /*
-    if(current >location[0]){
-        wall_follow2(LEFT,BACKWARD,TOOLS);
-        wait_ms(1000);
-        // back 
-        leftEncoder.reset();
-        rightEncoder.reset();
-        motors.setMotor0Speed(-MAX_SPEED); //right
-        motors.setMotor1Speed(-MAX_SPEED); //left
-        while(abs(leftEncoder.getPulses())<75 || abs(rightEncoder.getPulses())<75);
-        motors.stopBothMotors();
-        current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-    }
-    else{
-        wall_follow2(LEFT,FORWARD,TOOLS);   
-        // backward 
-        wait_ms(1000);
-        leftEncoder.reset();
-        rightEncoder.reset();
-        motors.setMotor0Speed(-MAX_SPEED); //right
-        motors.setMotor1Speed(-MAX_SPEED); //left
-        while(abs(leftEncoder.getPulses())<150 || abs(rightEncoder.getPulses())<150);
-        motors.stopBothMotors();
-        current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-    }*/
-    wait_ms(1000);
-    leftTurn();
-    
-    //Go over 
-    overBump();
-    
-}
\ No newline at end of file
+}   
\ No newline at end of file