Diff: main.cpp

Revision:

33:a41981e18a7d

Parent:

32:4a42f61f64a6

Child:

34:cd4980573159

diff -r 4a42f61f64a6 -r a41981e18a7d main.cpp
--- a/main.cpp	Mon Apr 21 19:23:26 2014 +0000
+++ b/main.cpp	Tue Apr 22 15:24:56 2014 +0000
@@ -118,7 +118,7 @@
 //Serial bt(p13,p14);
 HCSR04 rangeFinderFront( PIN_TRIGGERL, PIN_ECHOL );
 HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
-PID pid1(25.0,0.0,0.0,0.0001);
+PID pid1(25.0,2.0,1.0,0.0001);
 PololuQik2 motors(p9, p10, p8, p11, &errFunction, cRc);
 QEI rightEncoder(p22,p21,NC,PPR,QEI::X4_ENCODING);
 QEI leftEncoder(p23,p24,NC,PPR,QEI::X4_ENCODING);
@@ -166,7 +166,8 @@
 void overBump(int section);
 void alignWithWall(int section);
 void UntilWall(int dir);
-void alignWithGap();
+void alignWithGap(void);
+void alignWithRig(void);
 
 void testSensors(void);
 float normd(int* pop, int count, int threshold);
@@ -329,9 +330,9 @@
                 case START :
                     myled1 = 1;
 
-                    current=75;
-                    //state  = NAVIGATE_WAVES_ROW1;
-                    state = OILRIG1_POS;
+                    current=68;
+                    state  = NAVIGATE_WAVES_ROW1;
+                    //state = OILRIG1_POS;
                     break;
 
 
@@ -432,7 +433,7 @@
                     *
                     **************************************************/
                 case IDENTIFY_TOOLS:
-
+                
                     //wait(5);
                     servoPosition(TOOL_2);              //arm/camera looks over tool
                     wait(5);                            //wait for servos to settle
@@ -669,7 +670,7 @@
                     break;
 
                 case NAVIGATE_WAVES_ROW3:
-                    shape_detected = 1;
+                    tool_needed = 2;
                     if(tool_needed == 1) {
                         rig_section(location, current, direction, 1);
                         //state = NAVIGATE_TO_SQUARE_RIG;
@@ -1060,25 +1061,6 @@
         motors.stopBothMotors(127);
         wait_ms(300);
         return;
-        /*
-               rangeFinderRight.startMeas();
-               wait_ms(20);
-               rangeFinderRight.getMeas(range);
-
-               if(range>15){
-                   // turning left
-                   leftEncoder.reset();
-                   rightEncoder.reset();
-                   motors.setMotor0Speed(-MAX_SPEED); //right
-                   motors.setMotor1Speed(MAX_SPEED); //left
-                   while(abs(rightEncoder.getPulses()) < 50 || abs(leftEncoder.getPulses()) < 50);
-                   motors.stopBothMotors(127);
-                   return;
-               }
-        */
-        // turning left
-        //motors.setMotor0Speed(0.9*MAX_SPEED); //right
-        //motors.setMotor1Speed(-0.9*MAX_SPEED); //left
 
     } else if(section == RIGS) {
 
@@ -1098,76 +1080,23 @@
         motors.setMotor1Speed(-0.25*127); //left
         while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150);
         motors.stopBothMotors(127);
-        wait(2);
+        wait(1);
 
-        // turn right away from wall
-        leftEncoder.reset();
-        rightEncoder.reset();
-        motors.setMotor0Speed(-MAX_SPEED); //right
-        motors.setMotor1Speed(MAX_SPEED); //left
-        while(abs(rightEncoder.getPulses()) < 25 || abs(leftEncoder.getPulses()) < 25 );
-
+        if(IRLeftFront.getIRDistance() > 35 && IRLeftBack.getIRDistance() > 35) return;
+        // align with wave
+        while(IRLeftFront.getIRDistance() > 35) {
+            pc.printf("front sensor sees gap\r\n");
+            motors.setMotor0Speed(-0.1*127);//right
+            motors.setMotor1Speed(-0.1*127);//left
+        }
+        while(IRLeftBack.getIRDistance() >35 ) {
+            pc.printf("back sensor sees gap\r\n");
+            motors.setMotor0Speed(0.1*127);//right
+            motors.setMotor1Speed(0.1*127);//left
+        }
         motors.stopBothMotors(127);
 
-    } else if(section == MID2) {
-
-        // check distance to wall
-
-        if(IRLeftFront.getIRDistance() > 4) {
-
-            leftEncoder.reset();
-            rightEncoder.reset();
-            motors.setMotor0Speed(-1.2*MAX_SPEED); //right
-            motors.setMotor1Speed(0.4*MAX_SPEED); //left
-            while(rightEncoder.getPulses()>-1000);
-            motors.stopBothMotors(0);
-
-            //go backwards toward wall
-            leftEncoder.reset();
-            rightEncoder.reset();
-            motors.setMotor0Speed(-0.25*127); //right
-            motors.setMotor1Speed(-0.25*127); //left
-            while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150);
-
-            // turn left towards wall
-            leftEncoder.reset();
-            rightEncoder.reset();
-            motors.setMotor0Speed(0.4*127); //right
-            motors.setMotor1Speed(-0.4*127); //left
-            while(abs(rightEncoder.getPulses()) < 65 || abs(leftEncoder.getPulses()) < 65);
-            motors.stopBothMotors(127);
-
-            slightMove(FORWARD,100);
-        }
-        return;
-
-    } else { // MID
-        //pc.printf("in mid section align\r\n");
-        // turn right towards wall
-        rightTurn();
-        // turning left towards wall
-        //motors.setMotor0Speed(0.9*MAX_SPEED); //right
-        //motors.setMotor1Speed(-0.9*MAX_SPEED); //left
-
-    }
-
-    usValue = 0;
-    /*    while(1) {
-            if(section == 10) { // CURENTLY NOT USED (WAS RIGS)
-                rangeFinderRight.startMeas();
-                wait_ms(20);
-                rangeFinderRight.getMeas(range);
-            } else {
-                range = IRLeftFront.getIRDistance();
-            }
-            //pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
-            if(range > usValue && usValue != 0 && range < 25) {
-                break;
-            } else {
-                usValue = range;
-            }
-        }
-        motors.stopBothMotors(0);*/
+    } 
 }
 
 void rightTurn(void)
@@ -1607,12 +1536,18 @@
     if(rig == 1) loc= 15;
     else if(rig == 2) loc= 45;
     else loc = 75;
-
+    // Backwards until the gap is hit.
+    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(300);
+    
     // Slight forward for turn
     slightMove(FORWARD,150);
     wait_ms(100);
     leftTurn();
-    //slightright();
 
 
     if(current > loc) {
@@ -1625,12 +1560,11 @@
         current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     }
     wait(1);
-    alignWithWall(MID2);
-    current+=4;
     wall_follow2(LEFT, FORWARD, RIGS, current, rig);
     current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-    slightMove(FORWARD, 75);
-    current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+    wait(1);
+    
+    alignWithRig();
 }
 
 void tools_section_return(float* location, float &current)
@@ -1647,7 +1581,7 @@
 {
     if(direction[0] == RIGHT) {
         rightTurn();
-        alignWithWall(MID);
+        //alignWithWall(MID);
         wall_follow2(LEFT, FORWARD, MID, current,0);
         current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
         slightMove(FORWARD, 50);
@@ -1683,7 +1617,7 @@
 
 void rig_section_return(float* location, float &current, int* direction)
 {
-    alignWithWall(RIGS);
+    //alignWithWall(RIGS);
 
     if(location[2] > current) {
         wall_follow2(LEFT, BACKWARD, RETURN, current,0);
@@ -1811,29 +1745,29 @@
 void alignWithGap()
 {
     pc.printf("\r\n Align with gap.");
+    
     wait(1);
-    if(IRLeftBack.getIRDistance() > 35) {
+    
+    if(IRLeftBack.getIRDistance() > 30) {
         motors.setMotor0Speed(-0.1*127);
         motors.setMotor1Speed(-0.1*127);
-    }
-    while(IRLeftBack.getIRDistance() > 35);
-    pc.printf("IR left back %f \t",IRLeftBack.getIRDistance());
+        while(IRLeftBack.getIRDistance() > 30);
+        pc.printf("IR left back %f \t",IRLeftBack.getIRDistance());
 
-    motors.stopBothMotors(127);
-    wait_ms(200);
-    wait(1);
+        motors.stopBothMotors(127);
+        wait_ms(300);
+    }
 
-    if(IRLeftBack.getIRDistance() < 35) {
+
+    if(IRLeftBack.getIRDistance() < 30 && IRLeftFront.getIRDistance() > 30) {
         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());
+        while(IRLeftBack.getIRDistance() < 30);
 
-
-
-    motors.stopBothMotors(127);
-    wait_ms(200);
+        pc.printf("IR left back %f \t\r\n",IRLeftBack.getIRDistance());
+        motors.stopBothMotors(127);
+        wait_ms(300);
+    }
 
     leftEncoder.reset();
     rightEncoder.reset();
@@ -1841,9 +1775,32 @@
     motors.setMotor0Speed(0.1*127);
     motors.setMotor1Speed(0.1*127);
 
-    while(abs(leftEncoder.getPulses())<500 || rightEncoder.getPulses()<500);
+    while(abs(leftEncoder.getPulses())<575 || rightEncoder.getPulses()<575);
 
     motors.stopBothMotors(127);
-    wait_ms(200);
+    wait_ms(300);
+}
+
+void alignWithRig(){
+    
+    if(IRLeftFront.getIRDistance() < 30) {
+        motors.setMotor0Speed(0.1*127);
+        motors.setMotor1Speed(0.1*127);
+        while(IRLeftFront.getIRDistance() < 30);
+        pc.printf("IR left back %f \t",IRLeftFront.getIRDistance());
 
+        motors.stopBothMotors(127);
+        wait_ms(300);
+    }
+    
+    motors.setMotor0Speed(0.1*127);
+    motors.setMotor1Speed(0.1*127);
+
+    while(abs(leftEncoder.getPulses())<100 || rightEncoder.getPulses()<100);
+
+    motors.stopBothMotors(127);
+    wait_ms(300);
+
+
+    
 }
\ No newline at end of file