game.cpp

00001 #include "game.h"
00002 
00003 gamePlayer::gamePlayer(robot &botIn) : bot(botIn){
00004     row = 0;
00005     column = 0;
00006     rigOnFire=-1;
00007     waveGaps[0]=-1;
00008     waveGaps[1]=-1;
00009     waveGaps[2]=-1;
00010     direction = -1;
00011 }
00012 
00013 void gamePlayer::resetFirst(){
00014     row = 0;
00015     column = 0;
00016     rigOnFire=-1;
00017     waveGaps[0]=-1;
00018     waveGaps[1]=-1;
00019     waveGaps[2]=-1;
00020     direction = -1;
00021 }
00022 
00023 void gamePlayer::checkWaves(){
00024     direction = bot.gyro.getZDegrees();
00025     float oldWallDist;
00026     while(1){
00027         oldWallDist = bot.pingLeft.getStablePoll();
00028         if(oldWallDist>0.0 || oldWallDist<-1.5)
00029             break;
00030     }
00031     DBGPRINT("at %d distance %f\r\n",column,oldWallDist);
00032     float wallDist = -2;
00033     if(oldWallDist > 15.0 || oldWallDist < -1.5)
00034         waveGaps[row]=column;
00035     float desiredHeading=0;
00036     for(int i=column+1;i<6;i++){
00037         //move forward one wave worth
00038         bot.smoothMove(290*16,0,40);
00039         //straighten back out
00040         //DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
00041         //bot.smoothMove((8000.0/360.0)*(desiredHeading),1,40);
00042         while(1){
00043             wallDist = bot.pingLeft.getStablePoll();
00044             if(wallDist>0.0 || wallDist<-1.5)
00045                 break;
00046         }
00047         DBGPRINT("at %d distance %f\r\n",i,wallDist);
00048         if(wallDist > 15.0 || wallDist < -1.5){
00049             waveGaps[row]=i;
00050             DBGPRINT("See-Wall Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
00051             bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
00052             desiredHeading = 0;
00053         } else {
00054             //correct our direction
00055             if(!(waveGaps[row]==i-1) && i!=5){
00056                 float xDist = 16.0;
00057                 float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
00058                 desiredHeading = (atan2(3.9-wallDist,16.0)*180.0/3.14159)/2;
00059                 DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
00060                 DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
00061                 bot.smoothMove((8000.0/360.0)*((-heading+desiredHeading)),1,40);
00062             } else {
00063                 DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
00064                 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
00065                 desiredHeading = 0;
00066             }
00067             oldWallDist = wallDist;
00068         }
00069     }
00070     if(waveGaps[row]>=0)
00071         bot.smoothMove(-290*16*(5-waveGaps[row]),0,40);
00072 }
00073 
00074 void gamePlayer::runFirstRow(){
00075     if(row != 0){
00076         DBGPRINT("Not in first row for runFirstRow",1);
00077         return;
00078     }
00079     direction = bot.gyro.getZDegrees();
00080     float oldWallDist;
00081     while(1){
00082         oldWallDist = bot.pingLeft.getStablePoll();
00083         if(oldWallDist>0.0 || oldWallDist<-1.5)
00084             break;
00085     }
00086     waveGaps[row]=5;
00087     DBGPRINT("at %d distance %f\r\n",column,oldWallDist);
00088     float wallDist = -2;
00089     if(oldWallDist > 15.0 || oldWallDist < -1.5)
00090         waveGaps[row]=column;
00091     float desiredHeading=0;
00092     for(int i=column+1;i<5;i++){
00093         //move forward one wave worth
00094         bot.smoothMove(290*16,0,40);
00095         //straighten back out
00096         //DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
00097         //bot.smoothMove((8000.0/360.0)*(desiredHeading),1,40);
00098         while(1){
00099             wallDist = bot.pingLeft.getStablePoll();
00100             if(wallDist>0.0 || wallDist<-1.5)
00101                 break;
00102         }
00103         DBGPRINT("at %d distance %f\r\n",i,wallDist);
00104         if(i==3){
00105             bot.smoothMove((8000.0/360.0)*(-desiredHeading-90),1,40);
00106             int rigFound=0;
00107             while(1){
00108                 rigFound = bot.pollForRigs();
00109                 if(rigFound == 1 || rigFound == 2 || rigFound == 4)
00110                     break;
00111             }
00112             rigOnFire = rigFound;
00113             DBGPRINT("Found rig %d is on fire\r\n",rigFound);
00114             bot.smoothMove((8000.0/360.0)*(desiredHeading+90),1,40);
00115         }
00116         if(wallDist > 15.0 || wallDist < -1.5){
00117             waveGaps[row]=i;
00118             DBGPRINT("See-Wall Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
00119             bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
00120             desiredHeading = 0;
00121         } else {
00122             //correct our direction
00123             if(!(waveGaps[row]==i-1) && i!=4){
00124                 float xDist = 16.0;
00125                 float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
00126                 desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
00127                 DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
00128                 DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
00129                 bot.smoothMove((8000.0/360.0)*((-heading+desiredHeading)),1,40);
00130             } else {
00131                 DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
00132                 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
00133                 desiredHeading = 0;
00134             }
00135             oldWallDist = wallDist;
00136         }
00137     }
00138     //pick up procedure goes here
00139     bot.nearCamera();
00140     return;
00141     //move through gap
00142     if(waveGaps[row]>=0){
00143         bot.smoothMove(290*(16*(waveGaps[row]-4)+3),0,40);
00144         bot.smoothMove((8000.0/360.0)*(-90),1,40);
00145         if(waveGaps[row]==4){
00146             bot.smoothMove(290*(13+oldWallDist),0,40);
00147         } else {
00148             bot.smoothMove(290*(13+wallDist),0,40);
00149         }
00150         bot.smoothMove((8000.0/360.0)*(90),1,40);
00151         bot.smoothMove(290*(-3),0,40);
00152         column = waveGaps[row];
00153         row = 1;
00154     }
00155 }
00156 
00157 void gamePlayer::runSecondRow(){
00158     float gapCheck = -2;
00159     if(row != 1 || row != 2){
00160         DBGPRINT("Not in second row for runSecondRow",1);
00161         return;
00162     }
00163     direction = bot.gyro.getZDegrees();
00164     float oldWallDist;
00165     while(1){
00166         oldWallDist = bot.pingLeft.getStablePoll();
00167         if(oldWallDist>0.0 || oldWallDist<-1.5)
00168             break;
00169     }
00170     DBGPRINT("at %d distance %f\r\n",column,oldWallDist);
00171     float wallDist = -2;
00172     float desiredHeading=0;
00173     if(oldWallDist > 15.0 || oldWallDist < -1.5) {
00174         //found gap
00175         waveGaps[row]=column;
00176         //check if this is really the center of the gap
00177         bot.smoothMove(290*4,0,40);
00178         gapCheck = bot.pingLeft.getStablePoll();
00179         if((gapCheck > 15.0) || (gapCheck < -1.5)){
00180             bot.smoothMove(290*-4,0,40);
00181         }else{
00182             bot.smoothMove(290*-6,0,40);
00183         }
00184     } else {
00185         //check in one direction
00186         for(int i=column+1;i<6;i++){
00187             //move forward one wave worth
00188             bot.smoothMove(290*16,0,40);
00189             while(1){ //check distance to wall
00190                 wallDist = bot.pingLeft.getStablePoll();
00191                 if(wallDist>0.0 || wallDist<-1.5)
00192                     break;
00193             }
00194             DBGPRINT("at %d distance %f\r\n",i,wallDist);
00195             if(wallDist > 15.0 || wallDist < -1.5){
00196                 //found gap
00197                 waveGaps[row]=i;
00198              //check if this is really the center of the gap
00199                 bot.smoothMove(290*4,0,40);
00200                 gapCheck = bot.pingLeft.getStablePoll();
00201                 if((gapCheck > 15.0) || (gapCheck < -1.5)){
00202                     bot.smoothMove(290*-4,0,40);
00203                 }else{
00204                     bot.smoothMove(290*-6,0,40);
00205                 }
00206                 column = i;
00207                 DBGPRINT("See-Wall Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
00208                 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
00209                 desiredHeading = 0;
00210                 break;
00211             } else {
00212                 //correct our direction
00213                 if(!(waveGaps[row]==i-1) && i==5){
00214                     //we have enough info to correct direction
00215                     // and we are as far as we will go, so need to correct for and move a long trip
00216                     float xDist = 16.0;
00217                     float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
00218                     desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
00219                     DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
00220                     DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
00221                     bot.smoothMove((8000.0/360.0)*((-heading+desiredHeading)),1,40);
00222                 } else if(!(waveGaps[row]==i-1)){
00223                     //we have enough info to correct direction
00224                     // for the move to the next stop
00225                     float xDist = 16.0;
00226                     float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
00227                     desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
00228                     DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
00229                     DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
00230                     bot.smoothMove((8000.0/360.0)*((-heading+desiredHeading)),1,40);
00231                 } else {
00232                     // not enough info 
00233                     //  this should never happen because we are greedy and turn the first time we see a gap
00234                     DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
00235                     bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
00236                     desiredHeading = 0;
00237                 }
00238                 oldWallDist = wallDist;
00239             }
00240         }
00241         //haven't found, check other direction
00242         if(waveGaps[row]==-1){
00243             bot.smoothMove(290*(16*(column-5)),0,40);
00244             while(1){
00245             oldWallDist = bot.pingLeft.getStablePoll();
00246             if(oldWallDist>0.0 || oldWallDist<-1.5)
00247                 break;
00248             }
00249             DBGPRINT("at %d distance %f\r\n",column,oldWallDist);
00250 
00251             for(int i=column+1;i>0;i=i-1){
00252             //move backward one wave worth
00253             bot.smoothMove(290*-16,0,40);
00254             //straighten back out
00255             while(1){
00256                 wallDist = bot.pingLeft.getStablePoll();
00257                 if(wallDist>0.0 || wallDist<-1.5)
00258                     break;
00259             }
00260 
00261             DBGPRINT("at %d distance %f\r\n",i,wallDist);
00262             if(wallDist > 15.0 || wallDist < -1.5){
00263                 //found gap
00264                 waveGaps[row]=i;
00265                 column = i;
00266                 DBGPRINT("See-Wall Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
00267                 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
00268                 //check if this is really the center of the gap
00269                 bot.smoothMove(290*4,0,40);
00270                 gapCheck = bot.pingLeft.getStablePoll();
00271                 if((gapCheck > 15.0) || (gapCheck < -1.5)){
00272                     bot.smoothMove(290*-4,0,40);
00273                 }else{
00274                     bot.smoothMove(290*-6,0,40);
00275                 }
00276                 desiredHeading = 0;
00277                 break;
00278             } else {      
00279                 //correct our direction
00280                 if(!(waveGaps[row]==i-1) && i==5){
00281                     //we have enough info to correct direction
00282                     // and we are as far as we will go, so need to correct for and move a long trip
00283                     float xDist = 16.0;
00284                     float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
00285                     desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
00286                     DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
00287                     DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
00288                     bot.smoothMove((8000.0/360.0)*(-(-heading+desiredHeading)),1,40);
00289                 } else if(!(waveGaps[row]==i-1)){
00290                     //we have enough info to correct direction
00291                     // for the move to the next stop
00292                     float xDist = 16.0;
00293                     float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
00294                     desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
00295                     DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
00296                     DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
00297                     bot.smoothMove((8000.0/360.0)*(-(-heading+desiredHeading)),1,40);
00298                 } else {
00299                     // not enough info 
00300                     //  this should never happen because we are greedy and turn the first time we see a gap
00301                     DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
00302                     bot.smoothMove((8000.0/360.0)*(desiredHeading),1,40);
00303                     desiredHeading = 0;
00304                 }
00305                 oldWallDist = wallDist;
00306             }
00307         }
00308     }
00309     //move through gap
00310     if(waveGaps[row]>=0){
00311         //check if this is really the center of the gap
00312         bot.smoothMove(290*4,0,40);
00313         gapCheck = bot.pingLeft.getStablePoll();
00314         if((gapCheck > 15.0) || (gapCheck < -1.5)){
00315             bot.smoothMove(290*-4,0,40);
00316         }else{
00317             bot.smoothMove(290*-6,0,40);
00318         }
00319         //assumes we are already at the right column
00320         bot.smoothMove(290*(3),0,40);
00321         bot.smoothMove((8000.0/360.0)*(-90),1,40);
00322         if(waveGaps[row]==4){
00323             bot.smoothMove(290*(13+oldWallDist),0,40);
00324         } else {
00325             bot.smoothMove(290*(13+wallDist),0,40);
00326         }
00327         bot.smoothMove((8000.0/360.0)*(90),1,40);
00328         bot.smoothMove(290*(-3),0,40);
00329         column = waveGaps[row];
00330         row = 2;
00331     }
00332 }}
00333 
00334 void gamePlayer::approachRig(){
00335     while(1){ 
00336         if(rigOnFire == 1){   
00337             bot.smoothMove(290*(11.625-(column*16)),0,40);
00338             bot.smoothMove(290*(3),0,40);
00339             bot.smoothMove((8000.0/360.0)*(-90),1,40);
00340         } else if(rigOnFire == 2){
00341             bot.smoothMove(290*(40-(column*16)),0,40);
00342             bot.smoothMove(290*(3),0,40);
00343             bot.smoothMove((8000.0/360.0)*(-90),1,40);
00344         } else if(rigOnFire == 4){
00345             bot.smoothMove(290*(68.375-(column*16)),0,40);
00346             bot.smoothMove(290*(3),0,40);
00347             bot.smoothMove((8000.0/360.0)*(-90),1,40);    
00348         }
00349         break;
00350         }
00351 }