Revision 34:cd4980573159, committed 2014-04-24

Comitter:

Fairy_Paolina

Date:

Thu Apr 24 23:20:43 2014 +0000

Parent:

33:a41981e18a7d

Commit message:

Rolls up to the rig markings. ;

Changed in this revision

diff -r a41981e18a7d -r cd4980573159 main.cpp
--- a/main.cpp	Tue Apr 22 15:24:56 2014 +0000
+++ b/main.cpp	Thu Apr 24 23:20:43 2014 +0000
@@ -167,7 +167,8 @@
 void alignWithWall(int section);
 void UntilWall(int dir);
 void alignWithGap(void);
-void alignWithRig(void);
+void toRig(void);
+void alignWithRigWall(float &current);
 
 void testSensors(void);
 float normd(int* pop, int count, int threshold);
@@ -329,7 +330,8 @@
                     **************************************************/
                 case START :
                     myled1 = 1;
-
+                    //current=16;
+                    //state=NAVIGATE_WAVES_ROW3;
                     current=68;
                     state  = NAVIGATE_WAVES_ROW1;
                     //state = OILRIG1_POS;
@@ -747,7 +749,8 @@
                     *
                     **************************************************/
                 case RETURN_TO_START:
-                    wait(3);
+                    wait(5);
+                    while(1);
                     rig_section_return(location, current, direction);
                     mid_section2_return(location, current, direction);
                     mid_section_return(location, current, direction);
@@ -1524,7 +1527,8 @@
     motors.setMotor1Speed(-0.5*127);// left
     while(abs(leftEncoder.getPulses())<1000 || rightEncoder.getPulses()<1000);
     motors.stopBothMotors(127);
-
+    
+    slightMove(BACKWARD,50);
     overBump(RIGS);
     //pc.printf("overbump rigs\r\n");
 }
@@ -1545,26 +1549,30 @@
     wait_ms(300);
     
     // Slight forward for turn
-    slightMove(FORWARD,150);
+    slightMove(FORWARD,100);
     wait_ms(100);
-    leftTurn();
+    //RIGHT TURN
+    alignWithRigWall(current);
+    if(current <80) current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+    else current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
 
 
     if(current > loc) {
         //pc.printf("RIG section %f\r\n",current);
-        wall_follow2(LEFT, FORWARD, RIGS, current, rig);
+        wall_follow2(RIGHT, BACKWARD, RIGS, current, rig);
         current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     } else {
         //pc.printf("RIG section %f\r\n",current);
-        wall_follow2(LEFT, BACKWARD, RIGS, current, rig);
+        wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
         current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     }
-    wait(1);
-    wall_follow2(LEFT, FORWARD, RIGS, current, rig);
-    current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-    wait(1);
-    
-    alignWithRig();
+    wait_ms(500);
+    wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
+    current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+    wait_ms(500);
+    pc.printf("align with rig\r\n");
+    //toRig();
+    slightMove(FORWARD,785);
 }
 
 void tools_section_return(float* location, float &current)
@@ -1781,26 +1789,71 @@
     wait_ms(300);
 }
 
-void alignWithRig(){
+void toRig(){
     
-    if(IRLeftFront.getIRDistance() < 30) {
+    if(IRLeftFront.getIRDistance() < 38) {
         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);
+        while(IRLeftFront.getIRDistance() < 38);
+        pc.printf("IF\r\n");
     }
     
     motors.setMotor0Speed(0.1*127);
     motors.setMotor1Speed(0.1*127);
+    while(IRLeftBack.getIRDistance() > 38);
+    
+    //while(abs(leftEncoder.getPulses())<100 || rightEncoder.getPulses()<100);
+    // forward until back sensor does not see the rig
+    while(IRLeftBack.getIRDistance() < 38);
+    pc.printf("align\r\n");
+    motors.stopBothMotors(127);
+    wait_ms(300);
 
-    while(abs(leftEncoder.getPulses())<100 || rightEncoder.getPulses()<100);
+}
+
+void alignWithRigWall(float &current){
+    
+    motors.begin();
+    leftEncoder.reset();
+    rightEncoder.reset();
+    motors.setMotor0Speed(-0.5*127);//right
+    motors.setMotor1Speed(0.5*127);//left
+    while(abs(leftEncoder.getPulses())<950 || abs(rightEncoder.getPulses())<950);
+    motors.stopBothMotors(127);
+    wait_ms(300);
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
+    
+    if(current <80){
+        //forward until back sensor is in front of the wave
+        while(IRRightBack.getIRDistance() >35);
+        motors.setMotor0Speed(0.2*127);//right
+        motors.setMotor1Speed(0.2*127);//left
+    }
+    else{
+        //motors.setMotor0Speed(-0.1*127);//right
+        //motors.setMotor1Speed(-0.1*127);//left
+    }
+    
+    /*while(abs(leftEncoder.getPulses())<800 || abs(rightEncoder.getPulses())<800);
+    while(IRRightBack.getIRDistance() >35);
+ */  
+    motors.stopBothMotors(127);
+    wait_ms(300);
+
+    // Align with Gap
+    while(IRRightFront.getIRDistance() - IRRightBack.getIRDistance() > 0.5) {
+        pc.printf("turn right\r\n");
+        motors.setMotor0Speed(-0.3*127);//right
+        motors.setMotor1Speed(0.3*127);// left
+    }
+    while(IRRightFront.getIRDistance() - IRRightBack.getIRDistance() < -0.5) {
+        pc.printf("turn left\r\n");
+        motors.setMotor0Speed(0.3*127);// right
+        motors.setMotor1Speed(-0.3*127);// left
+    }
 
     motors.stopBothMotors(127);
     wait_ms(300);
-
-
-    
 }
\ No newline at end of file