Diff: game.cpp

Revision:

34:4ad11cda1eca

Parent:

33:03b0b66038e1

Child:

36:67773a788d52

--- a/game.cpp	Sun Mar 23 02:23:29 2014 +0000
+++ b/game.cpp	Sun Mar 23 08:24:07 2014 +0000
@@ -38,7 +38,7 @@
         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);
+            bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
             desiredHeading = 0;
         } else {
             //correct our direction
@@ -48,10 +48,10 @@
                 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);
+                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);
+                bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
                 desiredHeading = 0;
             }
             oldWallDist = wallDist;
@@ -59,4 +59,83 @@
     }
     if(waveGaps[row]>=0)
         bot.smoothMove(-290*16*(5-waveGaps[row]),0,40);
+}
+
+void gamePlayer::runFirstRow(){
+    if(row != 0){
+        DBGPRINT("Not in first row for runFirstRow",1);
+        return;
+    }
+    direction = bot.gyro.getZDegrees();
+    float oldWallDist;
+    while(1){
+        oldWallDist = bot.pingLeft.getStablePoll();
+        if(oldWallDist>0.0 || oldWallDist<-1.5)
+            break;
+    }
+    waveGaps[row]=5;
+    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<5;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(i==3){
+            bot.smoothMove((8000.0/360.0)*(-desiredHeading-90),1,40);
+            int rigFound=0;
+            while(1){
+                rigFound = bot.pollForRigs();
+                if(rigFound == 1 || rigFound == 2 || rigFound == 4)
+                    break;
+            }
+            rigOnFire = rigFound;
+            DBGPRINT("Found rig %d is on fire\r\n",rigFound);
+            bot.smoothMove((8000.0/360.0)*(desiredHeading+90),1,40);
+        }
+        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!=4){
+                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 {
+                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*(waveGaps[row]-4)+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