Diff: main.cpp

Revision:

31:99cf9c25b0f2

Parent:

30:db07aea6d119

Child:

32:4a42f61f64a6

diff -r db07aea6d119 -r 99cf9c25b0f2 main.cpp
--- a/main.cpp	Thu Apr 17 20:34:17 2014 +0000
+++ b/main.cpp	Mon Apr 21 18:27:12 2014 +0000
@@ -166,6 +166,7 @@
 void overBump(int section);
 void alignWithWall(int section);
 void UntilWall(int dir);
+void alignWithGap();
 
 void testSensors(void);
 float normd(int* pop, int count, int threshold);
@@ -211,7 +212,7 @@
 
 //increase in number 5 rotates gripper
 
-    {STORE_POSITION, 85, 5, 0, 170, 60, 0},              // storing position
+    {STORE_POSITION, 85, 8, 0, 170, 60, 0},              // storing position
     {OIL_RIG1, 164, 20, 60, 100, 175, 0},                 // point laser at oilrig1
     {OIL_RIG2, 164, 20, 60, 100, 175, 0},                // point laser at oilrig2
     {OIL_RIG3, 130, 90, 90, 100, 175, 0},                // NOT USED!!!!!    point laser at oilrig2
@@ -280,9 +281,9 @@
     motors.begin();
 
     startBtn.rise(&button_int);
- //////////////////////////// TEST SERVOS /////////////////////////
+//////////////////////////// TEST SERVOS /////////////////////////
     //testSensors();
-    
+
     motors.begin();
     //wall_follow2(LEFT, FORWARD, TOOLS, 10, 1);
     //while(1);
@@ -300,7 +301,7 @@
     while(1) {
         //if(1) {
         if(button_start == 1) {
-            
+
             /*
             pc.printf("Servo Test");
             while(1) {
@@ -329,8 +330,8 @@
                     myled1 = 1;
 
                     current=75;
-                    state  = NAVIGATE_WAVES_ROW1;
-                   //state = OILRIG1_POS;
+                    //state  = NAVIGATE_WAVES_ROW1;
+                    state = OILRIG1_POS;
                     break;
 
 
@@ -983,18 +984,18 @@
                 }
             }
         }
-        
+
         if(range > 40 && range2 > 40 && section != RIGS) {
-                pc.printf("RANGE %f \tRANGE3 %f\r\n",range,range3);
-                //STOP
-                motors.stopBothMotors(127);
-                break;
+            pc.printf("RANGE %f \tRANGE3 %f\r\n",range,range3);
+            //STOP
+            motors.stopBothMotors(127);
+            break;
 
         } else {
             if(direction == FORWARD) pid1.setProcessValue(range);
             else pid1.setProcessValue(range2);
 
-            if(abs(range - range2) < 10){ // does it see a wavegap? 
+            if(abs(range - range2) < 10) { // does it see a wavegap?
                 pid_return = pid1.compute();
                 //pc.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
 
@@ -1260,8 +1261,8 @@
     rightEncoder.reset();
     motors.setMotor0Speed(0.5*127);// right
     motors.setMotor1Speed(0.5*127);// left
-    while(abs(leftEncoder.getPulses())<50 || abs(rightEncoder.getPulses())<50){
-        
+    while(abs(leftEncoder.getPulses())<50 || abs(rightEncoder.getPulses())<50) {
+
     }
     motors.stopBothMotors(127);
 }
@@ -1344,7 +1345,7 @@
     wait_ms(200);
     motors.stopBothMotors(127);
     wait_ms(500);
-    
+
     if(section!= RIGS) {
         range = 0;
 
@@ -1356,8 +1357,8 @@
                 break;
             }
 
-            motors.setMotor1Speed(0.3*127);//left
-            motors.setMotor0Speed(0.3*127);//right
+            motors.setMotor1Speed(0.33*127);//left
+            motors.setMotor0Speed(0.33*127);//right
             wait_ms(220);
             motors.stopBothMotors(127);
             wait_ms(500);
@@ -1381,7 +1382,7 @@
             motors.setMotor1Speed(0.1*127);//left
             motors.setMotor0Speed(0.1*127);//right
             wait_ms(220);
-            
+
             if(motors.GetMotor0Current()>5 || motors.GetMotor1Current() >5) {
                 pc.printf("fast left %d \t right %d \r\n", motors.GetMotor0Current(), motors.GetMotor1Current());
                 motors.setMotor1Speed(0.35*127);//left
@@ -1429,9 +1430,15 @@
 
     if(IRLeftFront.getIRDistance() < 40 || IRLeftBack.getIRDistance() < 40) {
         wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
+        
         pc.printf("IR front %f \t back %f \r\n", IRLeftFront.getIRDistance(), IRLeftBack.getIRDistance());
         location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
         current= location[0];
+        /***********************************************
+        ALIGN with GAP
+        ***********************************************/
+        alignWithGap();
+
         //pc.printf("current %f \r\n",current);
         // go backwards
         //slightMove(BACKWARD,200);
@@ -1442,6 +1449,13 @@
     } else {
         pc.printf("IR front %f \t back %f \r\n", IRLeftFront.getIRDistance(), IRLeftBack.getIRDistance());
         //pc.printf("else greater than 20\r\n");
+        /***********************************************
+        ALIGN with GAP
+        ***********************************************/
+
+        alignWithGap();
+
+
         location[0]= current;
         leftTurn();
         overBump(TOOLS);
@@ -1455,6 +1469,7 @@
     wait_ms(500);
 
     if(IRFrontL.getIRDistance() > 40 && IRFrontR.getIRDistance() > 40) {
+
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(0.25*127); //right
@@ -1505,6 +1520,12 @@
     wait_ms(200);
     //pc.printf("wavegap2 = %f\r\n",location[1]);
     //left turn
+    /***********************************************
+        ALIGN with GAP
+    ***********************************************/
+
+    alignWithGap();
+
     motors.begin();
     leftEncoder.reset();
     rightEncoder.reset();
@@ -1560,14 +1581,19 @@
         current=location[2];
         //slightMove(BACKWARD,100);
     }
-
+    /***********************************************
+        ALIGN with GAP
+    ***********************************************/
+    alignWithGap();
     //LEFT turn
     motors.begin();
+    motors.stopBothMotors(127);
+    wait_ms(200);
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(0.5*127);// right
     motors.setMotor1Speed(-0.5*127);// left
-    while(abs(leftEncoder.getPulses())<1115 || rightEncoder.getPulses()<1115);
+    while(abs(leftEncoder.getPulses())<1000 || rightEncoder.getPulses()<1000);
     motors.stopBothMotors(127);
 
     overBump(RIGS);
@@ -1763,15 +1789,15 @@
 void testSensors(void)
 {
     float range, range2;
-    
+
 
     while(1) {
-        
+
         //pc.printf("us front %f \t us right %f \t IR front %f \t IR back %f\r\n", range, range2, IRLeftFront.getIRDistance(), IRLeftBack.getIRDistance());
-        
+
         pc.printf("IR LEFT front %f \t",IRLeftFront.getIRDistance());
         pc.printf("IR left back %f \t",IRLeftBack.getIRDistance());
-        
+
         pc.printf("IR right front %f \t",IRRightFront.getIRDistance());
         pc.printf("IR right back %f \t",IRRightBack.getIRDistance());
 
@@ -1779,4 +1805,45 @@
         pc.printf("IR front right %f \r\n",IRFrontR.getIRDistance());
         wait_ms(20);
     }
+}
+
+
+void alignWithGap()
+{
+    pc.printf("\r\n Align with gap.");
+    wait(1);
+    if(IRLeftBack.getIRDistance() > 35) {
+        motors.setMotor0Speed(-0.1*127);
+        motors.setMotor1Speed(-0.1*127);
+    }
+    while(IRLeftBack.getIRDistance() > 35);
+    pc.printf("IR left back %f \t",IRLeftBack.getIRDistance());
+
+    motors.stopBothMotors(127);
+    wait_ms(200);
+    wait(1);
+
+    if(IRLeftBack.getIRDistance() < 35) {
+        motors.setMotor0Speed(0.1*127);
+        motors.setMotor1Speed(0.1*127);
+    }
+    while(IRLeftBack.getIRDistance() < 35);
+    pc.printf("IR left back %f \t\r\n",IRLeftBack.getIRDistance());
+
+
+
+    motors.stopBothMotors(127);
+    wait_ms(200);
+
+    leftEncoder.reset();
+    rightEncoder.reset();
+
+    motors.setMotor0Speed(0.1*127);
+    motors.setMotor1Speed(0.1*127);
+
+    while(abs(leftEncoder.getPulses())<500 || rightEncoder.getPulses()<500);
+
+    motors.stopBothMotors(127);
+    wait_ms(200);
+
 }
\ No newline at end of file