Diff: game.cpp

Revision:

33:03b0b66038e1

Parent:

32:ff71f61bb9f6

Child:

34:4ad11cda1eca

--- a/game.cpp	Sat Mar 22 20:17:29 2014 +0000
+++ b/game.cpp	Sun Mar 23 02:23:29 2014 +0000
@@ -12,6 +12,51 @@
 
 void gamePlayer::checkWaves(){
     direction = bot.gyro.getZDegrees();
-    float wallDist = bot.pingLeft.getStablePoll();
-    
+    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;
+    if(oldWallDist > 15.0 || oldWallDist < -1.5)
+        waveGaps[row]=column;
+    float desiredHeading=0;
+    for(int i=column+1;i<6;i++){
+        //move forward one wave worth
+        bot.smoothMove(290*16,0,40);
+        //straighten back out
+        //DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
+        //bot.smoothMove((8000.0/360.0)*(desiredHeading),1,40);
+        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){
+            waveGaps[row]=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;
+        } else {
+            //correct our direction
+            if(!(waveGaps[row]==i-1) && i!=5){
+                float xDist = 16.0;
+                float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
+                desiredHeading = (atan2(3.9-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 {
+                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]>=0)
+        bot.smoothMove(-290*16*(5-waveGaps[row]),0,40);
 }
\ No newline at end of file