Diff: main.cpp

Revision:

24:3369d51f6cbd

Parent:

23:f8e806d1ffcc

Child:

26:7257bd16bc67

diff -r f8e806d1ffcc -r 3369d51f6cbd main.cpp
--- a/main.cpp	Sat Apr 12 20:27:00 2014 +0000
+++ b/main.cpp	Sat Apr 12 23:22:33 2014 +0000
@@ -272,6 +272,7 @@
  
  
     pc.baud(115200);
+    
     //Laser Range Finder Initialization
     lrf_baudCalibration();
  
@@ -303,7 +304,9 @@
                     **************************************************/
                 case START :
                     myled1 = 1;
-                    state = OILRIG1_POS;
+                    current=75;
+                    state  = NAVIGATE_WAVES_ROW1;
+                    //state = OILRIG1_POS;
                     break;
  
  
@@ -1247,7 +1250,7 @@
     rightEncoder.reset();
     motors.setMotor0Speed(-0.5*127);//right
     motors.setMotor1Speed(0.5*127);//left
-    while(abs(leftEncoder.getPulses())<850 || abs(rightEncoder.getPulses())<850);
+    while(abs(leftEncoder.getPulses())<800 || abs(rightEncoder.getPulses())<800);
     motors.stopBothMotors(127);
 }
  
@@ -1322,87 +1325,76 @@
  
 void overBump(int section)
 {
-    int preLeft=5000, preRight=5000, out=0;
- 
-    motors.begin();
-    // slight backwards
-    leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(-0.25*127); //right
-    motors.setMotor1Speed(-0.25*127); //left
-    while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50);
- 
-    motors.stopBothMotors(127);
- 
-    //pc.printf("slight backwards\r\n");
+    // first set
+    motors.setMotor1Speed(0.1*127);//left
+    motors.setMotor0Speed(0.1*127);//right
+    wait_ms(100);
+    while(motors.GetMotor0Current()<5 || motors.GetMotor1Current() <5 );
+
+    while(motors.GetMotor0Current()>5 || motors.GetMotor1Current() >5 ) {
+        pc.printf("current %f\r\n",IR.getIRDistance());
+        pc.printf("left %d \t right %d \r\n", motors.GetMotor0Current(), motors.GetMotor1Current());
+        motors.setMotor1Speed(0.3*127);//left
+        motors.setMotor0Speed(0.3*127);//right
+        wait_ms(220);
+        motors.stopBothMotors(127);
+        wait_ms(10);
+    }
+
+    motors.setMotor1Speed(0.1*127);//left
+    motors.setMotor0Speed(0.1*127);//right
+    wait_ms(100);
+    while(motors.GetMotor0Current()<5 || motors.GetMotor1Current() <5 );
+
+    motors.setMotor1Speed(0.3*127);//left
+    motors.setMotor0Speed(0.3*127);//right
+    // second set
     wait_ms(200);
- 
-    // Over bump
-    leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(0.33*127); //right
-    motors.setMotor1Speed(0.33*127); //left
-    while((abs(leftEncoder.getPulses()) < 900 || abs(rightEncoder.getPulses()) < 900) /*&& preLeft!=0*/ && IR.getIRDistance() >15 );
- 
- 
- 
-    //pc.printf("forward \r\n");
-    wait_ms(200);
- 
-    motors.stopBothMotors(0);
-    motors.begin();
- 
-    preLeft=preRight=5000 ;
-    leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(.25*127); //right
-    motors.setMotor1Speed(.25*127); //left
- 
-    if(section == TOOLS) {
-        while(IR.getIRDistance() > 10 && (abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
- 
-            if(IR.getIRDistance() > 38) break;
- 
-            preLeft=leftEncoder.getPulses();
-            preRight=rightEncoder.getPulses();
+    motors.stopBothMotors(127);
+    wait_ms(300);
+    if(section!= RIGS) {
+
+        while(IR.getIRDistance() >30) {
+            pc.printf("%f\r\n",IR.getIRDistance());
+            motors.setMotor1Speed(0.3*127);//left
+            motors.setMotor0Speed(0.3*127);//right
+            wait_ms(220);
+            motors.stopBothMotors(127);
             wait_ms(200);
         }
-    } else if(section == MID || section == MID2) {
-        if(section == MID2) {
-            motors.setMotor0Speed(.33*127); //right
-            motors.setMotor1Speed(.33*127); //left
-            while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400));
-        }
- 
-        while( IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) /*&& (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)*/) {
- 
-            //if(IR.getIRDistance() > 38) break;
- 
-            //preLeft=leftEncoder.getPulses();
-            //preRight=rightEncoder.getPulses();
-            //wait_ms(200);
-        }
- 
-    } else {// RIGS
-        while(abs(leftEncoder.getPulses()) < 220 || abs(rightEncoder.getPulses()) < 220);
- 
-        // go backwards to line up with bump
+        
+        motors.setMotor1Speed(0.1*127);//left
+        motors.setMotor0Speed(0.1*127);//right
+        wait_ms(100);
+        while(motors.GetMotor0Current()<5 || motors.GetMotor1Current() <5);
+        motors.stopBothMotors(127);
+        wait_ms(200);  
+        
+    } else {
         leftEncoder.reset();
         rightEncoder.reset();
- 
-        motors.setMotor0Speed(-.15*127); //right
-        motors.setMotor1Speed(-.15*127); //left
-        while((abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && preLeft!=0 ) {
-            preLeft = leftEncoder.getPulses();
-            preRight = rightEncoder.getPulses();
+
+    
+        while(leftEncoder.getPulses()<500 || rightEncoder.getPulses()<500) {
+            motors.setMotor1Speed(0.1*127);//left
+            motors.setMotor0Speed(0.1*127);//right
+            wait_ms(220);
+            if(motors.GetMotor0Current()>5 || motors.GetMotor1Current() >5){
+                motors.setMotor1Speed(0.3*127);//left
+                motors.setMotor0Speed(0.3*127);//right
+                wait_ms(220);
+                motors.stopBothMotors(127);
+                wait_ms(200);  
+                leftEncoder.reset();
+                rightEncoder.reset();
+            }
+            motors.stopBothMotors(127);
             wait_ms(200);
-            if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
         }
-        motors.stopBothMotors(127);
-        motors.begin();
- 
-        return;
     }
+
+
+            
  
     motors.stopBothMotors(127);
     wait_ms(20);
@@ -1547,13 +1539,7 @@
     //current-=4;
     //}
     rightTurn();
-    //right turn more
-    leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(-0.4*127);// right
-    motors.setMotor1Speed(0.4*127);// left
-    while(abs(leftEncoder.getPulses())<25 || abs(rightEncoder.getPulses())<25);
-    motors.stopBothMotors(127);
+
     //pc.printf("mid section current = %f\r\n",current);
     wall_follow2(LEFT,FORWARD,MID, current,0);
     current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);