Diff: game.cpp

Revision:

36:67773a788d52

Parent:

34:4ad11cda1eca

Child:

38:3062838df0ce

--- a/game.cpp	Sun Mar 23 22:03:26 2014 +0000
+++ b/game.cpp	Mon Mar 24 20:52:37 2014 +0000
@@ -125,6 +125,9 @@
             oldWallDist = wallDist;
         }
     }
+    //pick up procedure goes here
+    
+    //move through gap
     if(waveGaps[row]>=0){
         bot.smoothMove(290*(16*(waveGaps[row]-4)+3),0,40);
         bot.smoothMove((8000.0/360.0)*(-90),1,40);
@@ -138,4 +141,92 @@
         row = 1;
         column = waveGaps[row];
     }
+}
+
+void gamePlayer::runSecondRow(){
+    if(row != 1){
+        DBGPRINT("Not in secong row for runSecondRow",1);
+        return;
+    }
+    direction = bot.gyro.getZDegrees();
+    float oldWallDist;
+    while(1){
+        oldWallDist = bot.pingLeft.getStablePoll();
+        if(oldWallDist>0.0 || oldWallDist<-1.5)
+            break;
+    }
+    DBGPRINT("at %d distance %f\r\n",column,oldWallDist);
+    float wallDist = -2;
+    float desiredHeading=0;
+    if(oldWallDist > 15.0 || oldWallDist < -1.5) {
+        //found gap
+        waveGaps[row]=column;
+    } else {
+        //check in one direction
+        for(int i=column+1;i<6;i++){
+            //move forward one wave worth
+            bot.smoothMove(290*16,0,40);
+            while(1){ //check distance to wall
+                wallDist = bot.pingLeft.getStablePoll();
+                if(wallDist>0.0 || wallDist<-1.5)
+                    break;
+            }
+            DBGPRINT("at %d distance %f\r\n",i,wallDist);
+            if(wallDist > 15.0 || wallDist < -1.5){
+                //found gap
+                waveGaps[row]=i;
+                column = i;
+                DBGPRINT("See-Wall Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
+                bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
+                desiredHeading = 0;
+                break;
+            } else {
+                //correct our direction
+                if(!(waveGaps[row]==i-1) && i==5){
+                    //we have enough info to correct direction
+                    // and we are as far as we will go, so need to correct for and move a long trip
+                    float xDist = 16.0;
+                    float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
+                    desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
+                    DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
+                    DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
+                    bot.smoothMove((8000.0/360.0)*((-heading+desiredHeading)),1,40);
+                } else if(!(waveGaps[row]==i-1)){
+                    //we have enough info to correct direction
+                    // for the move to the next stop
+                    float xDist = 16.0;
+                    float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
+                    desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
+                    DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
+                    DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
+                    bot.smoothMove((8000.0/360.0)*((-heading+desiredHeading)),1,40);
+                } else {
+                    // not enough info 
+                    //  this should never happen because we are greedy and turn the first time we see a gap
+                    DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
+                    bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
+                    desiredHeading = 0;
+                }
+                oldWallDist = wallDist;
+            }
+        }
+        if(waveGaps[row]==-1){
+            // haven't found, check in the opposite direction
+        }
+    }
+    //move through gap
+    if(waveGaps[row]>=0){
+        //assumes we are already at the right column
+        bot.smoothMove(290*(3),0,40);
+        bot.smoothMove((8000.0/360.0)*(-90),1,40);
+        if(waveGaps[row]==4){
+            bot.smoothMove(290*(13+oldWallDist),0,40);
+        } else {
+            bot.smoothMove(290*(13+wallDist),0,40);
+        }
+        bot.smoothMove((8000.0/360.0)*(90),1,40);
+        bot.smoothMove(290*(-3),0,40);
+        row = 1;
+        column = waveGaps[row];
+    }
 }
\ No newline at end of file