Diff: main.cpp

Revision:

8:11ef93eebe07

Parent:

7:78745a518957

Child:

9:f34700716f1d

diff -r 78745a518957 -r 11ef93eebe07 main.cpp
--- a/main.cpp	Fri Mar 21 22:19:51 2014 +0000
+++ b/main.cpp	Sat Mar 22 21:47:14 2014 +0000
@@ -6,6 +6,7 @@
 #include "HCSR04.h"
 #include "stdio.h"
 #include "LPC17xx.h"
+#include "Sharp.h"
  
 #define PIN_TRIGGERL    (p12)
 #define PIN_ECHOL       (p11)
@@ -23,9 +24,10 @@
 #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);
@@ -40,6 +42,7 @@
 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
 QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
 QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
+Sharp IR(p20);
 //InterruptIn encoder(p29);
  
  
@@ -55,7 +58,7 @@
 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 overBump(int section);
 void alignWithWall(int section);
  
 //Variables
@@ -64,6 +67,7 @@
 {
     float location[3], current=0;
     int direction[3];
+    double distance;
  
     pc.baud(115200);
     bt.baud(115200);
@@ -74,8 +78,11 @@
     tools_section(location, current);
     mid_section(location, current, direction);
     mid_section2(location, current, direction);
- 
-   
+    /*while(1){
+        bt.printf("IR %f\r\n", IR.getDistance());
+        wait_ms(200);
+    }
+   */
  
 }
  
@@ -96,7 +103,7 @@
  
 float wall_follow(int side, int direction, int section)
 {
-    float location, wavegap=0, set=4;
+    float location, wavegap=0, set=5;
     int dir=1;
     
     pid1.reset();
@@ -117,12 +124,12 @@
         pid1.setSetPoint(set);
         if(side){
             rangeFinderLeft.startMeas();
-            wait_ms(20);
+            wait_ms(38);
             rangeFinderLeft.getMeas(range);
         }
         else{
             rangeFinderRight.startMeas();
-            wait_ms(20);
+            wait_ms(38);
             rangeFinderRight.getMeas(range);
             pc.printf("%d\r\n",range);
         }
@@ -171,7 +178,7 @@
 void wall_follow2(int side, int direction, int section, float location)
 {
     int SeeWaveGap = false, dir=1;
-    float set=4, loc=0;
+    float set=5, loc=0;
     
     pid1.reset();
     
@@ -181,7 +188,7 @@
     leftEncoder.reset();
     rightEncoder.reset();
         
-    while(loc + location < 80) {
+    while(loc + dir*location < 80) {
         loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
         
         pid1.setInputLimits(0.0, set);
@@ -190,12 +197,12 @@
  
         if(side){
             rangeFinderLeft.startMeas();
-            wait_ms(20);
+            wait_ms(38);
             rangeFinderLeft.getMeas(range);
         }
         else{
             rangeFinderRight.startMeas();
-            wait_ms(20);
+            wait_ms(38);
             rangeFinderRight.getMeas(range);
         }
  
@@ -204,7 +211,8 @@
         /* If after 20 ms the ultrasonic still sees 20+ cm */
         /*      then robot is at wave opening               */
  
-        pc.printf("range %f\r\n",range);
+        bt.printf("wall follow 2 range %f\r\n",range);
+        bt.printf("loc+location = %f\r\n", loc+location);
         if(range > 20) {
             motors.stopBothMotors();
             bt.printf("wavegap\r\n");
@@ -276,15 +284,7 @@
         motors.setMotor0Speed(0.7*MAX_SPEED); //right
         motors.setMotor1Speed(-0.7*MAX_SPEED); //left
     } else {
-        // turn right towards wall
-        leftEncoder.reset();
-        rightEncoder.reset();
-        motors.setMotor0Speed(-MAX_SPEED); //right
-        motors.setMotor1Speed(MAX_SPEED); //left
-        while(abs(rightEncoder.getPulses()) < 100);
-
-        motors.stopBothMotors();
-
+        rightTurn();
         motors.setMotor0Speed(-0.7*MAX_SPEED); //right
         motors.setMotor1Speed(0.7*MAX_SPEED); //left
     }
@@ -320,42 +320,68 @@
     rightEncoder.reset();
     motors.setMotor0Speed(0.4*127);// right
     motors.setMotor1Speed(-0.4*127);// left
-    while(abs(leftEncoder.getPulses())<1075 || rightEncoder.getPulses()<1075);
+    while(abs(leftEncoder.getPulses())<1000 || rightEncoder.getPulses()<1000);
     motors.stopBothMotors();
 }
  
-
-void overBump(int wave){
+ 
+void overBump(int section){
     int preLeft=5000, preRight=5000 ;
     
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(0.15*127); //right
     motors.setMotor1Speed(0.15*127); //left
-    while((abs(leftEncoder.getPulses()) < 700 || abs(rightEncoder.getPulses())< 700) && preLeft!=0){
+    while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){
+        preLeft=leftEncoder.getPulses();
+        preRight=rightEncoder.getPulses();
+        wait_ms(200);
+        bt.printf(" first while left %d right %d \r\n", preLeft, preRight);
+        if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
+    }
+    
+    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();
-        wait_ms(20);
-        if(leftEncoder.getPulses() == preLeft && rightEncoder.getPulses()== preRight) preLeft=preRight=0;
-    } 
+        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;
+    }
+    
+    motors.stopBothMotors();
     
-    if(wave == FAR){
-        while(leftEncoder.getPulses() != preLeft && rightEncoder.getPulses()!= preRight){
+    preLeft=preRight=5000 ;
+    leftEncoder.reset();
+    rightEncoder.reset();
+    motors.setMotor0Speed(MAX_SPEED); //right
+    motors.setMotor1Speed(MAX_SPEED); //left
+    
+    if(section == TOOLS){
+        while(IR.getDistance() > 20 ) {
+            bt.printf("IR %f\r\n", IR.getDistance());
+
             preLeft=leftEncoder.getPulses();
             preRight=rightEncoder.getPulses();
+            bt.printf("third while left %d right %d \r\n", preLeft, preRight);
             wait_ms(20);
-        }
-        
-        motors.stopBothMotors();
+            if(leftEncoder.getPulses() == preLeft && rightEncoder.getPulses()== preRight){
+                motors.setMotor0Speed(0.4*127); //right
+                motors.setMotor1Speed(0.4*127); //left
+            }
+        }  
     }
-        
-    leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(0.4*127); //right
-    motors.setMotor1Speed(0.4*127); //left
-    while(abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200);
-
+    else while((abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses())< 300));        
+    
     motors.stopBothMotors();
+    wait(2);
+    
 }
  
 void tools_section(float* location, float &current){
@@ -384,18 +410,8 @@
         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);
+        overBump(TOOLS);
     }
     else{   
         location[0]= 77;     
@@ -406,19 +422,12 @@
     
     bt.printf("wavegap = %f\r\n",location[0]);
 }
-
+ 
 void mid_section(float* location, float &current, int* direction){
     
     motors.begin();
     alignWithWall(MID);
- /*   
-    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();
-    */
+
     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);
@@ -438,7 +447,7 @@
     
     bt.printf("wavegap2 = %f\r\n",location[1]);
     leftTurn();    
-    overBump(FAR);
+    overBump(TOOLS);
     // go forward
     leftEncoder.reset();
     rightEncoder.reset();
@@ -452,7 +461,6 @@
 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);
@@ -470,10 +478,10 @@
     }
     
     leftTurn();    
-    overBump(FAR);
+    overBump(RIGS);
 }    
-
+ 
 void rig_section(float* location, float &current, int* direction, int rig){
     
     
-}   
\ No newline at end of file
+}
\ No newline at end of file