File content as of revision 47:bedbd8b05657:

#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;
        }
    }
    //pick up procedure goes here
    
    //move through gap
    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);
        column = waveGaps[row];
        row = 1;
    }
}

void gamePlayer::runSecondRow(){
    float gapCheck = -2;
    if(row != 1 || row != 2){
        DBGPRINT("Not in second row for runSecondRow",1);
        return;
    }
    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;
    float desiredHeading=0;
    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);
        gapCheck = 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++){
            //move forward one wave worth
            bot.smoothMove(290*16,0,40);
            while(1){ //check distance to wall
                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;
             //check if this is really the center of the gap
                bot.smoothMove(290*4,0,40);
                gapCheck = 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);
                desiredHeading = 0;
                break;
            } else {
                //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;
            }
        }
        //haven't found, check other direction
        if(waveGaps[row]==-1){
            bot.smoothMove(290*(16*(column-5)),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);
                gapCheck = 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 {      
                //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);
        gapCheck = 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);
        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);
        column = waveGaps[row];
        row = 2;
    }
}}

void gamePlayer::approachRig(){
    while(1){ 
        if(rigOnFire == 1){   
            bot.smoothMove(290*(11.625-(column*16)),0,40);
            bot.smoothMove(290*(3),0,40);
            bot.smoothMove((8000.0/360.0)*(-90),1,40);
        } else if(rigOnFire == 2){
            bot.smoothMove(290*(40-(column*16)),0,40);
            bot.smoothMove(290*(3),0,40);
            bot.smoothMove((8000.0/360.0)*(-90),1,40);
        } else if(rigOnFire == 4){
            bot.smoothMove(290*(68.375-(column*16)),0,40);
            bot.smoothMove(290*(3),0,40);
            bot.smoothMove((8000.0/360.0)*(-90),1,40);    
        }
        break;
        }
}