Diff: main.cpp

Revision:

4:f2333e66ec2c

Parent:

3:58726d2e11f0

Child:

5:70ccef3734ae

--- a/main.cpp	Tue Mar 18 17:47:45 2014 +0000
+++ b/main.cpp	Tue Mar 18 23:11:28 2014 +0000
@@ -16,8 +16,7 @@
 #define DIST_PER_PULSE (0.01054225722682)
 #define MTRS_TO_INCH   (39.3701)
 #define MAX_SPEED      (0.3*127)
-#define PPRL           (24)
-#define PPRR           (24)
+#define PPR            (4331/4)
 #define LEFT           (1)
 #define RIGHT          (0)
 #define FORWARD        (1)
@@ -37,15 +36,15 @@
 HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
 PID pid1(15.0,0.0,4.0,0.02);
 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
-QEI rightEncoder(p17,p18,NC,PPRR,QEI::X4_ENCODING);
-QEI leftEncoder(p16,p15,NC,PPRR,QEI::X4_ENCODING);
+QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
+QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
 //InterruptIn encoder(p29);
 
 
 //Functions
 
 float wall_follow(int side, int direction, int section);
-void wall_follow2(int side, int direction);
+void wall_follow2(int side, int direction, int section);
 void wall_follow3(int &currentLocation, int &WaveOpening);
 void leftTurn(void);
 void rightTurn(void);
@@ -65,20 +64,18 @@
     motors.begin();
     
     
-    leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(MAX_SPEED); //right
-    motors.setMotor1Speed(MAX_SPEED); //left
+    //leftEncoder.reset();
+    //rightEncoder.reset();
+    //motors.setMotor0Speed(MAX_SPEED); //right
+    //motors.setMotor1Speed(MAX_SPEED); //left
     
-    while((abs(leftEncoder.getPulses())/(44*PPRL) + abs(rightEncoder.getPulses())/(44*PPRR))/2 < 3);
+    //while((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/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);
-    /*
+    tools_section(location, current);
+ /*   
     //////////////////////////////// without predefined wavegaps//////////////////////////////////////////////
     current=0;
     if(location[0]< 75){
@@ -104,7 +101,7 @@
     // left or right
     
     
-    location[1]=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2;
+    location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
     
     
     
@@ -148,16 +145,18 @@
     float location, wavegap;
     int dir=1, set=5;
     
+    pid1.reset();
+    
     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;
+    location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
     
     while(location< 75) {
-        location=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2;
+        location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
         
         pid1.setInputLimits(0, set);
         pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
@@ -175,7 +174,7 @@
         }
         
         if(range > 20) {
-            wavegap=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2;
+            wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
             bt.printf("wavegap %f\r\n",wavegap);
             // AT WAVE OPENING!!!!
         }
@@ -211,11 +210,14 @@
 
 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
 
-void wall_follow2(int side, int direction)
+void wall_follow2(int side, int direction, int section)
 {
-    int SeeWaveGap = false, count=0, dir=1;
+    int SeeWaveGap = false, count=0, dir=1, set=5;
+    
+    pid1.reset();
     
     if(direction == BACKWARD) dir=-1;
+    if(section == TOOLS)set= 9;
     
     leftEncoder.reset();
     rightEncoder.reset();
@@ -359,47 +361,56 @@
     
     leftEncoder.reset();
     rightEncoder.reset();
+    motors.setMotor0Speed(127*.15); //right
+    motors.setMotor1Speed(127*.15); //left
+    while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <9);
+    
+    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()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <3);
+    
     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;
+    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);
     
      motors.stopBothMotors();
     ////////////////////////////////////////// determine shape and pick up tool ///////////////////////////////////////////////////////
-    /*
+    
     if(current >location[0]){
-        wall_follow2(LEFT,BACKWARD);
+        wall_follow2(LEFT,BACKWARD,TOOLS);
+        wait_ms(500);
         // 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();
+ /*       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;
+        wait_ms(500);*/
+        current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/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/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;*/
     }
     
     leftTurn();
     
     //Go over 
     overBump();
-    */
+    
 }