File content as of revision 34:4ad11cda1eca:

#include "game.h"

gamePlayer::gamePlayer(robot &botIn) : bot(botIn){
    row = 0;
    column = 0;
    rigOnFire=-1;
    waveGaps[0]=-1;
    waveGaps[1]=-1;
    waveGaps[2]=-1;
    direction = -1;
}

void gamePlayer::checkWaves(){
    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;
    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);
}

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];
    }
}