Diff: game.cpp

Revision:

38:3062838df0ce

Parent:

36:67773a788d52

Child:

39:e61dd30febfc

diff -r 38a4ad49343e -r 3062838df0ce game.cpp
--- a/game.cpp	Tue Mar 25 17:21:02 2014 +0000
+++ b/game.cpp	Fri Mar 28 21:53:04 2014 +0000
@@ -138,14 +138,15 @@
         }
         bot.smoothMove((8000.0/360.0)*(90),1,40);
         bot.smoothMove(290*(-3),0,40);
+        column = waveGaps[row];
         row = 1;
-        column = waveGaps[row];
     }
 }
 
 void gamePlayer::runSecondRow(){
+    float gapCheck = -2;
     if(row != 1){
-        DBGPRINT("Not in secong row for runSecondRow",1);
+        DBGPRINT("Not in second row for runSecondRow",1);
         return;
     }
     direction = bot.gyro.getZDegrees();
@@ -161,6 +162,14 @@
     if(oldWallDist > 15.0 || oldWallDist < -1.5) {
         //found gap
         waveGaps[row]=column;
+        //check if this is really the center of the gap
+        bot.smoothMove(290*4,0,40);
+        bot.pingLeft.getStablePoll();
+        if((gapCheck > 15.0) || (gapCheck < -1.5)){
+            bot.smoothMove(290*-4,0,40);
+        }else{
+            bot.smoothMove(290*-6,0,40);
+        }
     } else {
         //check in one direction
         for(int i=column+1;i<6;i++){
@@ -175,6 +184,14 @@
             if(wallDist > 15.0 || wallDist < -1.5){
                 //found gap
                 waveGaps[row]=i;
+             //check if this is really the center of the gap
+                bot.smoothMove(290*4,0,40);
+                bot.pingLeft.getStablePoll();
+                if((gapCheck > 15.0) || (gapCheck < -1.5)){
+                    bot.smoothMove(290*-4,0,40);
+                }else{
+                    bot.smoothMove(290*-6,0,40);
+                }
                 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);
@@ -210,12 +227,84 @@
                 oldWallDist = wallDist;
             }
         }
+        //haven't found, check other direction
         if(waveGaps[row]==-1){
-            // haven't found, check in the opposite direction
+            bot.smoothMove(290*(16*(column-5)+3),0,40);
+            while(1){
+            oldWallDist = bot.pingLeft.getStablePoll();
+            if(oldWallDist>0.0 || oldWallDist<-1.5)
+                break;
+            }
+            DBGPRINT("at %d distance %f\r\n",column,oldWallDist);
+
+            for(int i=column+1;i>0;i=i-1){
+            //move backward one wave worth
+            bot.smoothMove(290*-16,0,40);
+            //straighten back out
+            while(1){
+                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);
+                //check if this is really the center of the gap
+                bot.smoothMove(290*4,0,40);
+                bot.pingLeft.getStablePoll();
+                if((gapCheck > 15.0) || (gapCheck < -1.5)){
+                    bot.smoothMove(290*-4,0,40);
+                }else{
+                    bot.smoothMove(290*-6,0,40);
+                }
+                desiredHeading = 0;
+                break;
+            } /*else {      //need to reverse angles
+                //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;
+            }
         }
     }
     //move through gap
     if(waveGaps[row]>=0){
+        //check if this is really the center of the gap
+        bot.smoothMove(290*4,0,40);
+        bot.pingLeft.getStablePoll();
+        if((gapCheck > 15.0) || (gapCheck < -1.5)){
+            bot.smoothMove(290*-4,0,40);
+        }else{
+            bot.smoothMove(290*-6,0,40);
+        }
         //assumes we are already at the right column
         bot.smoothMove(290*(3),0,40);
         bot.smoothMove((8000.0/360.0)*(-90),1,40);