Diff: main.cpp

Revision:

7:0b7897232e93

Parent:

6:109f46d3cb96

Child:

8:32ba0fad1689

--- a/main.cpp	Thu Mar 20 17:21:28 2014 +0000
+++ b/main.cpp	Fri Mar 21 00:02:51 2014 +0000
@@ -34,7 +34,7 @@
 Serial pc(USBTX,USBRX);
 HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
 HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
-PID pid1(15.0,0.0,4.0,0.02);
+PID pid1(15.0,0.0,10.0,0.02);
 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);
@@ -51,6 +51,7 @@
 void us_distance(void);
 void tools_section(float *location, float &current);
 void overBump(void);
+void alignWithWall(void);
  
 //Variables
  
@@ -63,6 +64,66 @@
     bt.baud(115200);
     motors.begin();
     
+    /*****************************OLD CODE FOR GOING THROUGH WAVE SECTION****************/
+    /*
+    leftEncoder.reset();
+    rightEncoder.reset();
+    wall_follow2(LEFT,FORWARD,TOOLS);
+    
+    motors.stopBothMotors();
+    wait_ms(500);
+    leftEncoder.reset();
+    rightEncoder.reset();
+    
+    motors.setMotor0Speed(0.6*MAX_SPEED); //right
+    motors.setMotor1Speed(0.6*MAX_SPEED); //left
+    
+    wait_ms(200);
+    motors.stopBothMotors();
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
+    bt.printf("1st turn left\r\n");
+    
+    motors.setMotor0Speed(MAX_SPEED); //right
+    motors.setMotor1Speed(-MAX_SPEED); //left
+    
+    while(((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2) < 1);
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
+    
+    motors.setMotor0Speed(MAX_SPEED); //right
+    motors.setMotor1Speed(MAX_SPEED); //left
+    
+    while(((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/PPR)/2) < 2);
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
+    motors.setMotor0Speed(-MAX_SPEED); //right
+    motors.setMotor1Speed(MAX_SPEED); //left
+    float usValue = 0;
+    while(1){
+        rangeFinderLeft.startMeas();
+        wait_ms(100);
+        rangeFinderLeft.getMeas(range);
+        bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
+        if(range > usValue && usValue != 0 && range < 10){
+            break;
+        } else {
+            usValue = range;    
+        }
+    }
+    motors.stopBothMotors();
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
+    
+    wall_follow2(LEFT,FORWARD,TOOLS);
+    
+    while(1){}
+    */
+    /*
     //  Very Consistent Turn
     leftEncoder.reset();
     rightEncoder.reset();
@@ -81,9 +142,8 @@
             usValue = range;    
         }
     }
-    
+    */
     motors.stopBothMotors();
-    while(1){}
     /*
     
     leftEncoder.reset();
@@ -164,7 +224,7 @@
  
 void us_distance(void)
 {
-    pc.printf("Ultra Sonic\n\r");
+ 
     rangeFinderLeft.startMeas();
     wait_us(20);
     if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
@@ -180,7 +240,7 @@
     pid1.reset();
     
     if(direction == BACKWARD) dir=-1;
-    if(section == TOOLS)set= 5;
+    if(section == TOOLS)set= 10;
     
     leftEncoder.reset();
     rightEncoder.reset();
@@ -209,11 +269,12 @@
             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*MAX_SPEED);//left
             motors.setMotor0Speed(dir*MAX_SPEED);//right
-            motors.setMotor1Speed(dir*MAX_SPEED);//left
-            
+
         } else {
         
+        
         pid1.setProcessValue(range);
         pid_return = pid1.compute();
  
@@ -239,7 +300,8 @@
             motors.setMotor0Speed(dir*MAX_SPEED);//right
             motors.setMotor1Speed(dir*MAX_SPEED);//left
         }
-    }}
+        }
+        }
     return wavegap;
 }
  
@@ -247,7 +309,8 @@
  
 void wall_follow2(int side, int direction, int section)
 {
-    int SeeWaveGap = false, count=0, dir=1, set=5;
+    int SeeWaveGap = false, count=0, set=5;
+    float dir=1.0; 
     
     pid1.reset();
     
@@ -378,7 +441,7 @@
     rightEncoder.reset();
     motors.setMotor0Speed(-speedR*127);//right
     motors.setMotor1Speed(speedL*127);//left
-    while(leftEncoder.getPulses()<1050 || rightEncoder.getPulses()>-1050);
+    while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900);
     motors.stopBothMotors();
 }
  
@@ -388,7 +451,7 @@
     rightEncoder.reset();
     motors.setMotor0Speed(0.4*127);// right
     motors.setMotor1Speed(-0.4*127);// left
-    while(leftEncoder.getPulses()>-1200 || rightEncoder.getPulses()<1200);
+    while(leftEncoder.getPulses()>-940 || rightEncoder.getPulses()<940);
     motors.stopBothMotors();
 }
  
@@ -408,24 +471,90 @@
     */
     motors.stopBothMotors();
 }
+void alignWithWall(void){
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
+    
+    motors.setMotor0Speed(-MAX_SPEED); //right
+    motors.setMotor1Speed(0.4*MAX_SPEED); //left
+    while(rightEncoder.getPulses()>-1000);
+    motors.stopBothMotors();
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
+    
+    motors.setMotor0Speed(-MAX_SPEED); //right
+    motors.setMotor1Speed(-MAX_SPEED); //left
+    while(abs(leftEncoder.getPulses()*11.12/PPR) < 3);
+    
+    motors.setMotor0Speed(0.1*MAX_SPEED); //right
+    motors.setMotor1Speed(0.1*MAX_SPEED); //left
+    
+    wait_ms(200);
+    
+    motors.stopBothMotors();
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
+    
+    motors.setMotor0Speed(MAX_SPEED); //right
+    motors.setMotor1Speed(-MAX_SPEED); //left
+    while(rightEncoder.getPulses() < 260);
+    
+    motors.setMotor0Speed(0.7*MAX_SPEED); //right
+    motors.setMotor1Speed(-0.7*MAX_SPEED); //left
+    float usValue = 0;
+    while(1){
+        rangeFinderLeft.startMeas();
+        wait_ms(100);
+        rangeFinderLeft.getMeas(range);
+        bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
+        if(range > usValue && usValue != 0 && range < 25){
+            break;
+        } else {
+            usValue = range;    
+        }
+    }
+    
+    
+    
+    
+}
+
  
 void tools_section(float *location, float &current){
     
-    location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field
+    //location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field
+    //Too Far from wall to see gap :(
+    wall_follow(LEFT,FORWARD,TOOLS);
+    
     current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-    bt.printf("left Encoder : %d \t right Encoder %d \r\n",leftEncoder.getPulses(),rightEncoder.getPulses());
     
     bt.printf("wavegap %f \t current %f \r\n",location[0],current);
     
      motors.stopBothMotors();
     ////////////////////////////////////////// determine shape and pick up tool ///////////////////////////////////////////////////////
-    
-    if(current >location[0]){
+        alignWithWall();
+        wait_ms(200);
+        
+        rangeFinderLeft.startMeas();
+        wait_ms(100);
+        rangeFinderLeft.getMeas(range);
+        if(range < 10){
+        
         wall_follow2(LEFT,BACKWARD,TOOLS);
-        wait_ms(100);
+        
+        leftEncoder.reset();
+        rightEncoder.reset();
+        motors.setMotor0Speed(-0.9*MAX_SPEED); //right
+        motors.setMotor1Speed(-0.9*MAX_SPEED); //left
+        while(abs(leftEncoder.getPulses()*11.12/PPR) < 2);
+        motors.stopBothMotors();
+        
         // forward 
         
- /*       leftEncoder.reset();
+ /*     leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(-MAX_SPEED); //right
         motors.setMotor1Speed(-MAX_SPEED); //left
@@ -434,8 +563,7 @@
         wait_ms(500);*/
         current = (abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; 
         // REMOVED += because current should just be current pulses?
-    }
-    else{/*
+        /*
         wall_follow2(LEFT,FORWARD);   
         // backward 
         leftEncoder.reset();
@@ -445,12 +573,18 @@
         while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
         motors.stopBothMotors();
         current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;*/
-    }
+    
     
     leftTurn();
     
     //Go over 
     overBump();
+    } else { 
+    
+    leftTurn();
+    //Go over 
+    overBump();
+    }
     
 }
  
\ No newline at end of file