Diff: main.cpp

Revision:

3:58726d2e11f0

Parent:

2:3d0be48abcf2

Child:

4:f2333e66ec2c

diff -r 3d0be48abcf2 -r 58726d2e11f0 main.cpp
--- a/main.cpp	Sat Mar 15 22:37:06 2014 +0000
+++ b/main.cpp	Tue Mar 18 17:47:45 2014 +0000
@@ -16,10 +16,15 @@
 #define DIST_PER_PULSE (0.01054225722682)
 #define MTRS_TO_INCH   (39.3701)
 #define MAX_SPEED      (0.3*127)
-#define PPRL           (965)
-#define PPRR           (1075)
+#define PPRL           (24)
+#define PPRR           (24)
 #define LEFT           (1)
 #define RIGHT          (0)
+#define FORWARD        (1)
+#define BACKWARD       (0) 
+#define TOOLS          (0)
+#define MID            (1)
+#define RIGS           (2)  
 
 float range, pid_return;
 void errFunction(void);
@@ -39,53 +44,76 @@
 
 //Functions
 
-void wall_follow(int side);
-void wall_follow2(int side);
+float wall_follow(int side, int direction, int section);
+void wall_follow2(int side, int direction);
 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);
 
 //Variables
 
 int main(void)
 {
-    float location=0;
+    float location[3], current=0;
+    int direction[3];
 
     pc.baud(115200);
     bt.baud(115200);
     motors.begin();
     
-    //motors.setMotor0Speed(MAX_SPEED); //right
-    //motors.setMotor1Speed(MAX_SPEED); //left
-    
-    //motors.stopBothMotors();
-
-
-    //wall_follow(RIGHT);
-    leftEncoder.reset();
-    rightEncoder.reset();
-    wall_follow2(LEFT);
-    location=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2;
-     
-    leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(MAX_SPEED); //right
-    motors.setMotor1Speed(MAX_SPEED); //left
-    while(leftEncoder.getPulses()<300 || rightEncoder.getPulses()<300);
-    motors.stopBothMotors();
-    
-    leftTurn();
     
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(MAX_SPEED); //right
     motors.setMotor1Speed(MAX_SPEED); //left
-    while(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR)/2 <30);
+    
+    while((abs(leftEncoder.getPulses())/(44*PPRL) + abs(rightEncoder.getPulses())/(44*PPRR))/2 < 3);
+    
+    //while(((abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2 -3) < 20);
+    
     motors.stopBothMotors();
     
+    //Go to tools
+    //tools_section(location, current);
+    /*
+    //////////////////////////////// without predefined wavegaps//////////////////////////////////////////////
+    current=0;
+    if(location[0]< 75){
+        turnRight();
+        current=wall_follow(LEFT,FORWARD);
+        if(current == 0)turnLeft();
+        else{
+            direction[0]= RIGHT;
+            turnLeft();
+            overBump();
+        }
+    }
+    else if(location[0]>=75 || current == 0){  
+        turnLeft();
+        wall_follow2(RIGHT,FORWARD);  
+    }
+        
+    
+    
+    
+    
+    
+    // left or right
+    
+    
+    location[1]=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2;
+    
+    
+    
+    
+     
+    
+    
     leftTurn();
-    wall_follow2(RIGHT);
+    //wall_follow2(RIGHT);
     rightTurn();
     
 
@@ -96,7 +124,7 @@
 //   leftTurn();
 //   wait(1);
 //   rightTurn();
-
+*/
 
 }
 
@@ -115,13 +143,25 @@
     }
 }
 
-void wall_follow(int side)
+float wall_follow(int side, int direction, int section)
 {
-    while(1) {
-
-        pid1.setInputLimits(0, 5.0);
+    float location, wavegap;
+    int dir=1, set=5;
+    
+    if(direction == BACKWARD) dir=-1;
+    if(section == TOOLS)set= 9;
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
+    
+    location=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2;
+    
+    while(location< 75) {
+        location=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2;
+        
+        pid1.setInputLimits(0, set);
         pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
-        pid1.setSetPoint(5.0);
+        pid1.setSetPoint(set);
         if(side){
             rangeFinderLeft.startMeas();
             wait_ms(20);
@@ -134,40 +174,51 @@
             pc.printf("%d\r\n",range);
         }
         
+        if(range > 20) {
+            wavegap=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2;
+            bt.printf("wavegap %f\r\n",wavegap);
+            // AT WAVE OPENING!!!!
+        }
+        
         pid1.setProcessValue(range);
         pid_return = pid1.compute();
 
         if(pid_return > 0) {
             if(side){
-                motors.setMotor0Speed(MAX_SPEED - pid_return);//right
-                motors.setMotor1Speed(MAX_SPEED);//left
+                motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
+                motors.setMotor1Speed(dir*MAX_SPEED);//left
             }
             else{
-                motors.setMotor1Speed(MAX_SPEED - pid_return);//left
-                motors.setMotor0Speed(MAX_SPEED);//right
+                motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
+                motors.setMotor0Speed(dir*MAX_SPEED);//right
             }
         }else if(pid_return < 0) {
             if(side){
-                motors.setMotor0Speed(MAX_SPEED);//right
-                motors.setMotor1Speed(MAX_SPEED + pid_return);//left
+                motors.setMotor0Speed(dir*MAX_SPEED);//right
+                motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
             }
             else{
-                motors.setMotor1Speed(MAX_SPEED);//left
-                motors.setMotor0Speed(MAX_SPEED + pid_return);//right
+                motors.setMotor1Speed(dir*MAX_SPEED);//left
+                motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
             }
         }else {
-            motors.setMotor0Speed(MAX_SPEED);//right
-            motors.setMotor1Speed(MAX_SPEED);//left
+            motors.setMotor0Speed(dir*MAX_SPEED);//right
+            motors.setMotor1Speed(dir*MAX_SPEED);//left
         }
     }
+    return wavegap;
 }
 
 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
 
-void wall_follow2(int side)
+void wall_follow2(int side, int direction)
 {
-    int SeeWaveGap = false;
-    int count=0;
+    int SeeWaveGap = false, count=0, dir=1;
+    
+    if(direction == BACKWARD) dir=-1;
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
 
     while(1) {
 
@@ -188,7 +239,7 @@
 
 
         /*************CHECK FOR WAVE OPENING*****************/
-        /* If after 60 ms the ultrasonic still sees 20+ cm */
+        /* If after 20 ms the ultrasonic still sees 20+ cm */
         /*      then robot is at wave opening               */
 
         pc.printf("range %f\r\n",range);
@@ -205,25 +256,25 @@
 
                if(pid_return > 0) {
             if(side){
-                motors.setMotor0Speed(MAX_SPEED - pid_return);//right
-                motors.setMotor1Speed(MAX_SPEED);//left
+                motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
+                motors.setMotor1Speed(dir*MAX_SPEED);//left
             }
             else{
-                motors.setMotor1Speed(MAX_SPEED - pid_return);//left
-                motors.setMotor0Speed(MAX_SPEED);//right
+                motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
+                motors.setMotor0Speed(dir*MAX_SPEED);//right
             }
         }else if(pid_return < 0) {
             if(side){
-                motors.setMotor0Speed(MAX_SPEED);//right
-                motors.setMotor1Speed(MAX_SPEED + pid_return);//left
+                motors.setMotor0Speed(dir*MAX_SPEED);//right
+                motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
             }
             else{
-                motors.setMotor1Speed(MAX_SPEED);//left
-                motors.setMotor0Speed(MAX_SPEED + pid_return);//right
+                motors.setMotor1Speed(dir*MAX_SPEED);//left
+                motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
             }
         } else {
-            motors.setMotor0Speed(MAX_SPEED);
-            motors.setMotor1Speed(MAX_SPEED);
+            motors.setMotor0Speed(dir*MAX_SPEED);
+            motors.setMotor1Speed(dir*MAX_SPEED);
         }
     }
 }
@@ -303,3 +354,52 @@
     while(leftEncoder.getPulses()>-1200 || rightEncoder.getPulses()<1200);
     motors.stopBothMotors();
 }
+
+void overBump(void){
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
+    motors.setMotor0Speed(MAX_SPEED); //right
+    motors.setMotor1Speed(MAX_SPEED); //left
+    while(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR)/2 <30);
+    motors.stopBothMotors();
+}
+
+void tools_section(float* location, float &current){
+    
+    location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field
+    current=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2;
+    bt.printf("wavegap %f \t current %f \r\n",location[0],current);
+    
+     motors.stopBothMotors();
+    ////////////////////////////////////////// determine shape and pick up tool ///////////////////////////////////////////////////////
+    /*
+    if(current >location[0]){
+        wall_follow2(LEFT,BACKWARD);
+        // forward 
+        leftEncoder.reset();
+        rightEncoder.reset();
+        motors.setMotor0Speed(MAX_SPEED); //right
+        motors.setMotor1Speed(MAX_SPEED); //left
+        while(leftEncoder.getPulses()<300 || rightEncoder.getPulses()<300);
+        motors.stopBothMotors();
+        current+=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2;
+    }
+    else{
+        wall_follow2(LEFT,FORWARD);   
+        // backward 
+        leftEncoder.reset();
+        rightEncoder.reset();
+        motors.setMotor0Speed(-MAX_SPEED); //right
+        motors.setMotor1Speed(-MAX_SPEED); //left
+        while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
+        motors.stopBothMotors();
+        current-=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2;
+    }
+    
+    leftTurn();
+    
+    //Go over 
+    overBump();
+    */
+}