Sooner Competitive Robotics / Mbed 2 deprecated IEEE_14_Freescale

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of IEEE_14_Freescale by IEEE 2014 Mbed

game.cpp@36:67773a788d52, 2014-03-24 (annotated)

Committer:
sswatek
Date:
Mon Mar 24 20:52:37 2014 +0000
Revision:
36:67773a788d52
Parent:
34:4ad11cda1eca
Child:
38:3062838df0ce
Shell of code for second row traversal

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sswatek 32:ff71f61bb9f6 1 #include "game.h"
sswatek 32:ff71f61bb9f6 2
sswatek 32:ff71f61bb9f6 3 gamePlayer::gamePlayer(robot &botIn) : bot(botIn){
sswatek 32:ff71f61bb9f6 4 row = 0;
sswatek 32:ff71f61bb9f6 5 column = 0;
sswatek 32:ff71f61bb9f6 6 rigOnFire=-1;
sswatek 32:ff71f61bb9f6 7 waveGaps[0]=-1;
sswatek 32:ff71f61bb9f6 8 waveGaps[1]=-1;
sswatek 32:ff71f61bb9f6 9 waveGaps[2]=-1;
sswatek 32:ff71f61bb9f6 10 direction = -1;
sswatek 32:ff71f61bb9f6 11 }
sswatek 32:ff71f61bb9f6 12
sswatek 32:ff71f61bb9f6 13 void gamePlayer::checkWaves(){
sswatek 32:ff71f61bb9f6 14 direction = bot.gyro.getZDegrees();
sswatek 33:03b0b66038e1 15 float oldWallDist;
sswatek 33:03b0b66038e1 16 while(1){
sswatek 33:03b0b66038e1 17 oldWallDist = bot.pingLeft.getStablePoll();
sswatek 33:03b0b66038e1 18 if(oldWallDist>0.0 || oldWallDist<-1.5)
sswatek 33:03b0b66038e1 19 break;
sswatek 33:03b0b66038e1 20 }
sswatek 33:03b0b66038e1 21 DBGPRINT("at %d distance %f\r\n",column,oldWallDist);
sswatek 33:03b0b66038e1 22 float wallDist = -2;
sswatek 33:03b0b66038e1 23 if(oldWallDist > 15.0 || oldWallDist < -1.5)
sswatek 33:03b0b66038e1 24 waveGaps[row]=column;
sswatek 33:03b0b66038e1 25 float desiredHeading=0;
sswatek 33:03b0b66038e1 26 for(int i=column+1;i<6;i++){
sswatek 33:03b0b66038e1 27 //move forward one wave worth
sswatek 33:03b0b66038e1 28 bot.smoothMove(290*16,0,40);
sswatek 33:03b0b66038e1 29 //straighten back out
sswatek 33:03b0b66038e1 30 //DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 33:03b0b66038e1 31 //bot.smoothMove((8000.0/360.0)*(desiredHeading),1,40);
sswatek 33:03b0b66038e1 32 while(1){
sswatek 33:03b0b66038e1 33 wallDist = bot.pingLeft.getStablePoll();
sswatek 33:03b0b66038e1 34 if(wallDist>0.0 || wallDist<-1.5)
sswatek 33:03b0b66038e1 35 break;
sswatek 33:03b0b66038e1 36 }
sswatek 33:03b0b66038e1 37 DBGPRINT("at %d distance %f\r\n",i,wallDist);
sswatek 33:03b0b66038e1 38 if(wallDist > 15.0 || wallDist < -1.5){
sswatek 33:03b0b66038e1 39 waveGaps[row]=i;
sswatek 33:03b0b66038e1 40 DBGPRINT("See-Wall Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 34:4ad11cda1eca 41 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
sswatek 33:03b0b66038e1 42 desiredHeading = 0;
sswatek 33:03b0b66038e1 43 } else {
sswatek 33:03b0b66038e1 44 //correct our direction
sswatek 33:03b0b66038e1 45 if(!(waveGaps[row]==i-1) && i!=5){
sswatek 33:03b0b66038e1 46 float xDist = 16.0;
sswatek 33:03b0b66038e1 47 float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
sswatek 33:03b0b66038e1 48 desiredHeading = (atan2(3.9-wallDist,16.0)*180.0/3.14159)/2;
sswatek 33:03b0b66038e1 49 DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
sswatek 33:03b0b66038e1 50 DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
sswatek 34:4ad11cda1eca 51 bot.smoothMove((8000.0/360.0)*((-heading+desiredHeading)),1,40);
sswatek 33:03b0b66038e1 52 } else {
sswatek 33:03b0b66038e1 53 DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 34:4ad11cda1eca 54 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
sswatek 33:03b0b66038e1 55 desiredHeading = 0;
sswatek 33:03b0b66038e1 56 }
sswatek 33:03b0b66038e1 57 oldWallDist = wallDist;
sswatek 33:03b0b66038e1 58 }
sswatek 33:03b0b66038e1 59 }
sswatek 33:03b0b66038e1 60 if(waveGaps[row]>=0)
sswatek 33:03b0b66038e1 61 bot.smoothMove(-290*16*(5-waveGaps[row]),0,40);
sswatek 34:4ad11cda1eca 62 }
sswatek 34:4ad11cda1eca 63
sswatek 34:4ad11cda1eca 64 void gamePlayer::runFirstRow(){
sswatek 34:4ad11cda1eca 65 if(row != 0){
sswatek 34:4ad11cda1eca 66 DBGPRINT("Not in first row for runFirstRow",1);
sswatek 34:4ad11cda1eca 67 return;
sswatek 34:4ad11cda1eca 68 }
sswatek 34:4ad11cda1eca 69 direction = bot.gyro.getZDegrees();
sswatek 34:4ad11cda1eca 70 float oldWallDist;
sswatek 34:4ad11cda1eca 71 while(1){
sswatek 34:4ad11cda1eca 72 oldWallDist = bot.pingLeft.getStablePoll();
sswatek 34:4ad11cda1eca 73 if(oldWallDist>0.0 || oldWallDist<-1.5)
sswatek 34:4ad11cda1eca 74 break;
sswatek 34:4ad11cda1eca 75 }
sswatek 34:4ad11cda1eca 76 waveGaps[row]=5;
sswatek 34:4ad11cda1eca 77 DBGPRINT("at %d distance %f\r\n",column,oldWallDist);
sswatek 34:4ad11cda1eca 78 float wallDist = -2;
sswatek 34:4ad11cda1eca 79 if(oldWallDist > 15.0 || oldWallDist < -1.5)
sswatek 34:4ad11cda1eca 80 waveGaps[row]=column;
sswatek 34:4ad11cda1eca 81 float desiredHeading=0;
sswatek 34:4ad11cda1eca 82 for(int i=column+1;i<5;i++){
sswatek 34:4ad11cda1eca 83 //move forward one wave worth
sswatek 34:4ad11cda1eca 84 bot.smoothMove(290*16,0,40);
sswatek 34:4ad11cda1eca 85 //straighten back out
sswatek 34:4ad11cda1eca 86 //DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 34:4ad11cda1eca 87 //bot.smoothMove((8000.0/360.0)*(desiredHeading),1,40);
sswatek 34:4ad11cda1eca 88 while(1){
sswatek 34:4ad11cda1eca 89 wallDist = bot.pingLeft.getStablePoll();
sswatek 34:4ad11cda1eca 90 if(wallDist>0.0 || wallDist<-1.5)
sswatek 34:4ad11cda1eca 91 break;
sswatek 34:4ad11cda1eca 92 }
sswatek 34:4ad11cda1eca 93 DBGPRINT("at %d distance %f\r\n",i,wallDist);
sswatek 34:4ad11cda1eca 94 if(i==3){
sswatek 34:4ad11cda1eca 95 bot.smoothMove((8000.0/360.0)*(-desiredHeading-90),1,40);
sswatek 34:4ad11cda1eca 96 int rigFound=0;
sswatek 34:4ad11cda1eca 97 while(1){
sswatek 34:4ad11cda1eca 98 rigFound = bot.pollForRigs();
sswatek 34:4ad11cda1eca 99 if(rigFound == 1 || rigFound == 2 || rigFound == 4)
sswatek 34:4ad11cda1eca 100 break;
sswatek 34:4ad11cda1eca 101 }
sswatek 34:4ad11cda1eca 102 rigOnFire = rigFound;
sswatek 34:4ad11cda1eca 103 DBGPRINT("Found rig %d is on fire\r\n",rigFound);
sswatek 34:4ad11cda1eca 104 bot.smoothMove((8000.0/360.0)*(desiredHeading+90),1,40);
sswatek 34:4ad11cda1eca 105 }
sswatek 34:4ad11cda1eca 106 if(wallDist > 15.0 || wallDist < -1.5){
sswatek 34:4ad11cda1eca 107 waveGaps[row]=i;
sswatek 34:4ad11cda1eca 108 DBGPRINT("See-Wall Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 34:4ad11cda1eca 109 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
sswatek 34:4ad11cda1eca 110 desiredHeading = 0;
sswatek 34:4ad11cda1eca 111 } else {
sswatek 34:4ad11cda1eca 112 //correct our direction
sswatek 34:4ad11cda1eca 113 if(!(waveGaps[row]==i-1) && i!=4){
sswatek 34:4ad11cda1eca 114 float xDist = 16.0;
sswatek 34:4ad11cda1eca 115 float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
sswatek 34:4ad11cda1eca 116 desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
sswatek 34:4ad11cda1eca 117 DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
sswatek 34:4ad11cda1eca 118 DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
sswatek 34:4ad11cda1eca 119 bot.smoothMove((8000.0/360.0)*((-heading+desiredHeading)),1,40);
sswatek 34:4ad11cda1eca 120 } else {
sswatek 34:4ad11cda1eca 121 DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 34:4ad11cda1eca 122 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
sswatek 34:4ad11cda1eca 123 desiredHeading = 0;
sswatek 34:4ad11cda1eca 124 }
sswatek 34:4ad11cda1eca 125 oldWallDist = wallDist;
sswatek 34:4ad11cda1eca 126 }
sswatek 34:4ad11cda1eca 127 }
sswatek 36:67773a788d52 128 //pick up procedure goes here
sswatek 36:67773a788d52 129
sswatek 36:67773a788d52 130 //move through gap
sswatek 34:4ad11cda1eca 131 if(waveGaps[row]>=0){
sswatek 34:4ad11cda1eca 132 bot.smoothMove(290*(16*(waveGaps[row]-4)+3),0,40);
sswatek 34:4ad11cda1eca 133 bot.smoothMove((8000.0/360.0)*(-90),1,40);
sswatek 34:4ad11cda1eca 134 if(waveGaps[row]==4){
sswatek 34:4ad11cda1eca 135 bot.smoothMove(290*(13+oldWallDist),0,40);
sswatek 34:4ad11cda1eca 136 } else {
sswatek 34:4ad11cda1eca 137 bot.smoothMove(290*(13+wallDist),0,40);
sswatek 34:4ad11cda1eca 138 }
sswatek 34:4ad11cda1eca 139 bot.smoothMove((8000.0/360.0)*(90),1,40);
sswatek 34:4ad11cda1eca 140 bot.smoothMove(290*(-3),0,40);
sswatek 34:4ad11cda1eca 141 row = 1;
sswatek 34:4ad11cda1eca 142 column = waveGaps[row];
sswatek 34:4ad11cda1eca 143 }
sswatek 36:67773a788d52 144 }
sswatek 36:67773a788d52 145
sswatek 36:67773a788d52 146 void gamePlayer::runSecondRow(){
sswatek 36:67773a788d52 147 if(row != 1){
sswatek 36:67773a788d52 148 DBGPRINT("Not in secong row for runSecondRow",1);
sswatek 36:67773a788d52 149 return;
sswatek 36:67773a788d52 150 }
sswatek 36:67773a788d52 151 direction = bot.gyro.getZDegrees();
sswatek 36:67773a788d52 152 float oldWallDist;
sswatek 36:67773a788d52 153 while(1){
sswatek 36:67773a788d52 154 oldWallDist = bot.pingLeft.getStablePoll();
sswatek 36:67773a788d52 155 if(oldWallDist>0.0 || oldWallDist<-1.5)
sswatek 36:67773a788d52 156 break;
sswatek 36:67773a788d52 157 }
sswatek 36:67773a788d52 158 DBGPRINT("at %d distance %f\r\n",column,oldWallDist);
sswatek 36:67773a788d52 159 float wallDist = -2;
sswatek 36:67773a788d52 160 float desiredHeading=0;
sswatek 36:67773a788d52 161 if(oldWallDist > 15.0 || oldWallDist < -1.5) {
sswatek 36:67773a788d52 162 //found gap
sswatek 36:67773a788d52 163 waveGaps[row]=column;
sswatek 36:67773a788d52 164 } else {
sswatek 36:67773a788d52 165 //check in one direction
sswatek 36:67773a788d52 166 for(int i=column+1;i<6;i++){
sswatek 36:67773a788d52 167 //move forward one wave worth
sswatek 36:67773a788d52 168 bot.smoothMove(290*16,0,40);
sswatek 36:67773a788d52 169 while(1){ //check distance to wall
sswatek 36:67773a788d52 170 wallDist = bot.pingLeft.getStablePoll();
sswatek 36:67773a788d52 171 if(wallDist>0.0 || wallDist<-1.5)
sswatek 36:67773a788d52 172 break;
sswatek 36:67773a788d52 173 }
sswatek 36:67773a788d52 174 DBGPRINT("at %d distance %f\r\n",i,wallDist);
sswatek 36:67773a788d52 175 if(wallDist > 15.0 || wallDist < -1.5){
sswatek 36:67773a788d52 176 //found gap
sswatek 36:67773a788d52 177 waveGaps[row]=i;
sswatek 36:67773a788d52 178 column = i;
sswatek 36:67773a788d52 179 DBGPRINT("See-Wall Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 36:67773a788d52 180 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
sswatek 36:67773a788d52 181 desiredHeading = 0;
sswatek 36:67773a788d52 182 break;
sswatek 36:67773a788d52 183 } else {
sswatek 36:67773a788d52 184 //correct our direction
sswatek 36:67773a788d52 185 if(!(waveGaps[row]==i-1) && i==5){
sswatek 36:67773a788d52 186 //we have enough info to correct direction
sswatek 36:67773a788d52 187 // and we are as far as we will go, so need to correct for and move a long trip
sswatek 36:67773a788d52 188 float xDist = 16.0;
sswatek 36:67773a788d52 189 float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
sswatek 36:67773a788d52 190 desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
sswatek 36:67773a788d52 191 DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
sswatek 36:67773a788d52 192 DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
sswatek 36:67773a788d52 193 bot.smoothMove((8000.0/360.0)*((-heading+desiredHeading)),1,40);
sswatek 36:67773a788d52 194 } else if(!(waveGaps[row]==i-1)){
sswatek 36:67773a788d52 195 //we have enough info to correct direction
sswatek 36:67773a788d52 196 // for the move to the next stop
sswatek 36:67773a788d52 197 float xDist = 16.0;
sswatek 36:67773a788d52 198 float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
sswatek 36:67773a788d52 199 desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
sswatek 36:67773a788d52 200 DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
sswatek 36:67773a788d52 201 DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
sswatek 36:67773a788d52 202 bot.smoothMove((8000.0/360.0)*((-heading+desiredHeading)),1,40);
sswatek 36:67773a788d52 203 } else {
sswatek 36:67773a788d52 204 // not enough info
sswatek 36:67773a788d52 205 // this should never happen because we are greedy and turn the first time we see a gap
sswatek 36:67773a788d52 206 DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 36:67773a788d52 207 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
sswatek 36:67773a788d52 208 desiredHeading = 0;
sswatek 36:67773a788d52 209 }
sswatek 36:67773a788d52 210 oldWallDist = wallDist;
sswatek 36:67773a788d52 211 }
sswatek 36:67773a788d52 212 }
sswatek 36:67773a788d52 213 if(waveGaps[row]==-1){
sswatek 36:67773a788d52 214 // haven't found, check in the opposite direction
sswatek 36:67773a788d52 215 }
sswatek 36:67773a788d52 216 }
sswatek 36:67773a788d52 217 //move through gap
sswatek 36:67773a788d52 218 if(waveGaps[row]>=0){
sswatek 36:67773a788d52 219 //assumes we are already at the right column
sswatek 36:67773a788d52 220 bot.smoothMove(290*(3),0,40);
sswatek 36:67773a788d52 221 bot.smoothMove((8000.0/360.0)*(-90),1,40);
sswatek 36:67773a788d52 222 if(waveGaps[row]==4){
sswatek 36:67773a788d52 223 bot.smoothMove(290*(13+oldWallDist),0,40);
sswatek 36:67773a788d52 224 } else {
sswatek 36:67773a788d52 225 bot.smoothMove(290*(13+wallDist),0,40);
sswatek 36:67773a788d52 226 }
sswatek 36:67773a788d52 227 bot.smoothMove((8000.0/360.0)*(90),1,40);
sswatek 36:67773a788d52 228 bot.smoothMove(290*(-3),0,40);
sswatek 36:67773a788d52 229 row = 1;
sswatek 36:67773a788d52 230 column = waveGaps[row];
sswatek 36:67773a788d52 231 }
sswatek 32:ff71f61bb9f6 232 }