Diff: main.cpp

Revision:

19:9329e9be4c66

Parent:

18:a0ea7ecaf4fe

Child:

20:b9cbaf7566e9

--- a/main.cpp	Thu Apr 03 22:41:24 2014 +0000
+++ b/main.cpp	Thu Apr 03 23:01:50 2014 +0000
@@ -1006,7 +1006,7 @@
 void wall_follow2(int side, int direction, int section, float location, int rig)
 {
     int dir=1, limit=86, lowlim=5;
-    float set=7, loc=0, Rigloc=0;
+    float set=6, loc=0, Rigloc=0;
     bool SeeWaveGap = false;
 
     if(rig == 1) Rigloc= 16;
@@ -1023,7 +1023,8 @@
         set= 6;
         limit = 86;
     }
-    if(section == RETURN) lowlim=15;
+    else if(section == RIGS) set = 3;
+    else if(section == RETURN) lowlim=15;
 
     leftEncoder.reset();
     rightEncoder.reset();
@@ -1180,19 +1181,20 @@
         motors.stopBothMotors(0);
         wait(2);
 
-        // turn left towards wall
+        // turn right towards wall
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(-MAX_SPEED); //right
         motors.setMotor1Speed(MAX_SPEED); //left
-        while(abs(rightEncoder.getPulses()) < 20 || abs(leftEncoder.getPulses()) < 20);
+        while(abs(rightEncoder.getPulses()) < 50 || abs(leftEncoder.getPulses()) < 50);
 
-        motors.stopBothMotors(0);
-        wait(2);
+        motors.stopBothMotors(127);
+/*        wait(2);
 
         // turning left
         motors.setMotor0Speed(-0.9*MAX_SPEED); //right
         motors.setMotor1Speed(0.9*MAX_SPEED); //left
+*/
     } else {// MID
         pc.printf("in mid section align\r\n");
         // turn right towards wall
@@ -1204,7 +1206,7 @@
 
     usValue = 0;
     while(1) {
-        if(section == RIGS) {
+        if(section == 10) { // CURENTLY NOT USED (WAS RIGS)
             rangeFinderRight.startMeas();
             wait_ms(20);
             rangeFinderRight.getMeas(range);
@@ -1621,6 +1623,11 @@
     }
 
     alignWithWall(RIGS);
+    // Go forward until Rig
+    wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
+    // line back wheel up with side of rig
+    slightMove(FORWARD,300);
+    current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
 }
 
 void tools_section_return(float* location, float &current)