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@47:bedbd8b05657, 2014-04-03 (annotated)

Committer:
soonerbot
Date:
Thu Apr 03 02:54:41 2014 +0000
Revision:
47:bedbd8b05657
Parent:
46:faad265d8ce9 
added parenthesis

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);
soonerbot 38:3062838df0ce 141 column = waveGaps[row];
sswatek 34:4ad11cda1eca 142 row = 1;
sswatek 34:4ad11cda1eca 143 }
sswatek 36:67773a788d52 144 }
sswatek 36:67773a788d52 145
sswatek 36:67773a788d52 146 void gamePlayer::runSecondRow(){
soonerbot 38:3062838df0ce 147 float gapCheck = -2;
soonerbot 45:9c520ebc38e0 148 if(row != 1 || row != 2){
soonerbot 38:3062838df0ce 149 DBGPRINT("Not in second row for runSecondRow",1);
sswatek 36:67773a788d52 150 return;
sswatek 36:67773a788d52 151 }
sswatek 36:67773a788d52 152 direction = bot.gyro.getZDegrees();
sswatek 36:67773a788d52 153 float oldWallDist;
sswatek 36:67773a788d52 154 while(1){
sswatek 36:67773a788d52 155 oldWallDist = bot.pingLeft.getStablePoll();
sswatek 36:67773a788d52 156 if(oldWallDist>0.0 || oldWallDist<-1.5)
sswatek 36:67773a788d52 157 break;
sswatek 36:67773a788d52 158 }
sswatek 36:67773a788d52 159 DBGPRINT("at %d distance %f\r\n",column,oldWallDist);
sswatek 36:67773a788d52 160 float wallDist = -2;
sswatek 36:67773a788d52 161 float desiredHeading=0;
sswatek 36:67773a788d52 162 if(oldWallDist > 15.0 || oldWallDist < -1.5) {
sswatek 36:67773a788d52 163 //found gap
sswatek 36:67773a788d52 164 waveGaps[row]=column;
soonerbot 38:3062838df0ce 165 //check if this is really the center of the gap
soonerbot 38:3062838df0ce 166 bot.smoothMove(290*4,0,40);
soonerbot 39:e61dd30febfc 167 gapCheck = bot.pingLeft.getStablePoll();
soonerbot 38:3062838df0ce 168 if((gapCheck > 15.0) || (gapCheck < -1.5)){
soonerbot 38:3062838df0ce 169 bot.smoothMove(290*-4,0,40);
soonerbot 38:3062838df0ce 170 }else{
soonerbot 38:3062838df0ce 171 bot.smoothMove(290*-6,0,40);
soonerbot 38:3062838df0ce 172 }
sswatek 36:67773a788d52 173 } else {
sswatek 36:67773a788d52 174 //check in one direction
sswatek 36:67773a788d52 175 for(int i=column+1;i<6;i++){
sswatek 36:67773a788d52 176 //move forward one wave worth
sswatek 36:67773a788d52 177 bot.smoothMove(290*16,0,40);
sswatek 36:67773a788d52 178 while(1){ //check distance to wall
sswatek 36:67773a788d52 179 wallDist = bot.pingLeft.getStablePoll();
sswatek 36:67773a788d52 180 if(wallDist>0.0 || wallDist<-1.5)
sswatek 36:67773a788d52 181 break;
sswatek 36:67773a788d52 182 }
sswatek 36:67773a788d52 183 DBGPRINT("at %d distance %f\r\n",i,wallDist);
sswatek 36:67773a788d52 184 if(wallDist > 15.0 || wallDist < -1.5){
sswatek 36:67773a788d52 185 //found gap
sswatek 36:67773a788d52 186 waveGaps[row]=i;
soonerbot 38:3062838df0ce 187 //check if this is really the center of the gap
soonerbot 38:3062838df0ce 188 bot.smoothMove(290*4,0,40);
soonerbot 39:e61dd30febfc 189 gapCheck = bot.pingLeft.getStablePoll();
soonerbot 38:3062838df0ce 190 if((gapCheck > 15.0) || (gapCheck < -1.5)){
soonerbot 38:3062838df0ce 191 bot.smoothMove(290*-4,0,40);
soonerbot 38:3062838df0ce 192 }else{
soonerbot 38:3062838df0ce 193 bot.smoothMove(290*-6,0,40);
soonerbot 38:3062838df0ce 194 }
sswatek 36:67773a788d52 195 column = i;
sswatek 36:67773a788d52 196 DBGPRINT("See-Wall Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 36:67773a788d52 197 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
sswatek 36:67773a788d52 198 desiredHeading = 0;
sswatek 36:67773a788d52 199 break;
sswatek 36:67773a788d52 200 } else {
sswatek 36:67773a788d52 201 //correct our direction
sswatek 36:67773a788d52 202 if(!(waveGaps[row]==i-1) && i==5){
sswatek 36:67773a788d52 203 //we have enough info to correct direction
sswatek 36:67773a788d52 204 // and we are as far as we will go, so need to correct for and move a long trip
sswatek 36:67773a788d52 205 float xDist = 16.0;
sswatek 36:67773a788d52 206 float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
sswatek 36:67773a788d52 207 desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
sswatek 36:67773a788d52 208 DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
sswatek 36:67773a788d52 209 DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
sswatek 36:67773a788d52 210 bot.smoothMove((8000.0/360.0)*((-heading+desiredHeading)),1,40);
sswatek 36:67773a788d52 211 } else if(!(waveGaps[row]==i-1)){
sswatek 36:67773a788d52 212 //we have enough info to correct direction
sswatek 36:67773a788d52 213 // for the move to the next stop
sswatek 36:67773a788d52 214 float xDist = 16.0;
sswatek 36:67773a788d52 215 float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
sswatek 36:67773a788d52 216 desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
sswatek 36:67773a788d52 217 DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
sswatek 36:67773a788d52 218 DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
sswatek 36:67773a788d52 219 bot.smoothMove((8000.0/360.0)*((-heading+desiredHeading)),1,40);
sswatek 36:67773a788d52 220 } else {
sswatek 36:67773a788d52 221 // not enough info
sswatek 36:67773a788d52 222 // this should never happen because we are greedy and turn the first time we see a gap
sswatek 36:67773a788d52 223 DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 36:67773a788d52 224 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
sswatek 36:67773a788d52 225 desiredHeading = 0;
sswatek 36:67773a788d52 226 }
sswatek 36:67773a788d52 227 oldWallDist = wallDist;
sswatek 36:67773a788d52 228 }
sswatek 36:67773a788d52 229 }
soonerbot 38:3062838df0ce 230 //haven't found, check other direction
sswatek 36:67773a788d52 231 if(waveGaps[row]==-1){
soonerbot 39:e61dd30febfc 232 bot.smoothMove(290*(16*(column-5)),0,40);
soonerbot 38:3062838df0ce 233 while(1){
soonerbot 38:3062838df0ce 234 oldWallDist = bot.pingLeft.getStablePoll();
soonerbot 38:3062838df0ce 235 if(oldWallDist>0.0 || oldWallDist<-1.5)
soonerbot 38:3062838df0ce 236 break;
soonerbot 38:3062838df0ce 237 }
soonerbot 38:3062838df0ce 238 DBGPRINT("at %d distance %f\r\n",column,oldWallDist);
soonerbot 38:3062838df0ce 239
soonerbot 38:3062838df0ce 240 for(int i=column+1;i>0;i=i-1){
soonerbot 38:3062838df0ce 241 //move backward one wave worth
soonerbot 38:3062838df0ce 242 bot.smoothMove(290*-16,0,40);
soonerbot 38:3062838df0ce 243 //straighten back out
soonerbot 38:3062838df0ce 244 while(1){
soonerbot 38:3062838df0ce 245 wallDist = bot.pingLeft.getStablePoll();
soonerbot 38:3062838df0ce 246 if(wallDist>0.0 || wallDist<-1.5)
soonerbot 38:3062838df0ce 247 break;
soonerbot 38:3062838df0ce 248 }
soonerbot 38:3062838df0ce 249
soonerbot 38:3062838df0ce 250 DBGPRINT("at %d distance %f\r\n",i,wallDist);
soonerbot 38:3062838df0ce 251 if(wallDist > 15.0 || wallDist < -1.5){
soonerbot 38:3062838df0ce 252 //found gap
soonerbot 38:3062838df0ce 253 waveGaps[row]=i;
soonerbot 38:3062838df0ce 254 column = i;
soonerbot 38:3062838df0ce 255 DBGPRINT("See-Wall Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
soonerbot 38:3062838df0ce 256 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
soonerbot 38:3062838df0ce 257 //check if this is really the center of the gap
soonerbot 38:3062838df0ce 258 bot.smoothMove(290*4,0,40);
soonerbot 39:e61dd30febfc 259 gapCheck = bot.pingLeft.getStablePoll();
soonerbot 38:3062838df0ce 260 if((gapCheck > 15.0) || (gapCheck < -1.5)){
soonerbot 38:3062838df0ce 261 bot.smoothMove(290*-4,0,40);
soonerbot 38:3062838df0ce 262 }else{
soonerbot 38:3062838df0ce 263 bot.smoothMove(290*-6,0,40);
soonerbot 38:3062838df0ce 264 }
soonerbot 38:3062838df0ce 265 desiredHeading = 0;
soonerbot 38:3062838df0ce 266 break;
soonerbot 45:9c520ebc38e0 267 } else {
soonerbot 38:3062838df0ce 268 //correct our direction
soonerbot 38:3062838df0ce 269 if(!(waveGaps[row]==i-1) && i==5){
soonerbot 38:3062838df0ce 270 //we have enough info to correct direction
soonerbot 38:3062838df0ce 271 // and we are as far as we will go, so need to correct for and move a long trip
soonerbot 38:3062838df0ce 272 float xDist = 16.0;
soonerbot 38:3062838df0ce 273 float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
soonerbot 38:3062838df0ce 274 desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
soonerbot 38:3062838df0ce 275 DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
soonerbot 38:3062838df0ce 276 DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
soonerbot 45:9c520ebc38e0 277 bot.smoothMove((8000.0/360.0)*(-(-heading+desiredHeading)),1,40);
soonerbot 38:3062838df0ce 278 } else if(!(waveGaps[row]==i-1)){
soonerbot 38:3062838df0ce 279 //we have enough info to correct direction
soonerbot 38:3062838df0ce 280 // for the move to the next stop
soonerbot 38:3062838df0ce 281 float xDist = 16.0;
soonerbot 38:3062838df0ce 282 float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
soonerbot 38:3062838df0ce 283 desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
soonerbot 38:3062838df0ce 284 DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
soonerbot 38:3062838df0ce 285 DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
soonerbot 45:9c520ebc38e0 286 bot.smoothMove((8000.0/360.0)*(-(-heading+desiredHeading)),1,40);
soonerbot 38:3062838df0ce 287 } else {
soonerbot 38:3062838df0ce 288 // not enough info
soonerbot 38:3062838df0ce 289 // this should never happen because we are greedy and turn the first time we see a gap
soonerbot 38:3062838df0ce 290 DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
soonerbot 45:9c520ebc38e0 291 bot.smoothMove((8000.0/360.0)*(desiredHeading),1,40);
soonerbot 38:3062838df0ce 292 desiredHeading = 0;
soonerbot 45:9c520ebc38e0 293 }
soonerbot 38:3062838df0ce 294 oldWallDist = wallDist;
soonerbot 38:3062838df0ce 295 }
sswatek 36:67773a788d52 296 }
sswatek 36:67773a788d52 297 }
sswatek 36:67773a788d52 298 //move through gap
sswatek 36:67773a788d52 299 if(waveGaps[row]>=0){
soonerbot 38:3062838df0ce 300 //check if this is really the center of the gap
soonerbot 38:3062838df0ce 301 bot.smoothMove(290*4,0,40);
soonerbot 39:e61dd30febfc 302 gapCheck = bot.pingLeft.getStablePoll();
soonerbot 38:3062838df0ce 303 if((gapCheck > 15.0) || (gapCheck < -1.5)){
soonerbot 38:3062838df0ce 304 bot.smoothMove(290*-4,0,40);
soonerbot 38:3062838df0ce 305 }else{
soonerbot 38:3062838df0ce 306 bot.smoothMove(290*-6,0,40);
soonerbot 38:3062838df0ce 307 }
sswatek 36:67773a788d52 308 //assumes we are already at the right column
sswatek 36:67773a788d52 309 bot.smoothMove(290*(3),0,40);
sswatek 36:67773a788d52 310 bot.smoothMove((8000.0/360.0)*(-90),1,40);
sswatek 36:67773a788d52 311 if(waveGaps[row]==4){
sswatek 36:67773a788d52 312 bot.smoothMove(290*(13+oldWallDist),0,40);
sswatek 36:67773a788d52 313 } else {
sswatek 36:67773a788d52 314 bot.smoothMove(290*(13+wallDist),0,40);
sswatek 36:67773a788d52 315 }
sswatek 36:67773a788d52 316 bot.smoothMove((8000.0/360.0)*(90),1,40);
sswatek 36:67773a788d52 317 bot.smoothMove(290*(-3),0,40);
sswatek 36:67773a788d52 318 column = waveGaps[row];
soonerbot 45:9c520ebc38e0 319 row = 2;
sswatek 36:67773a788d52 320 }
soonerbot 45:9c520ebc38e0 321 }}
soonerbot 45:9c520ebc38e0 322
soonerbot 45:9c520ebc38e0 323 void gamePlayer::approachRig(){
soonerbot 45:9c520ebc38e0 324 while(1){
soonerbot 45:9c520ebc38e0 325 if(rigOnFire == 1){
soonerbot 47:bedbd8b05657 326 bot.smoothMove(290*(11.625-(column*16)),0,40);
soonerbot 45:9c520ebc38e0 327 bot.smoothMove(290*(3),0,40);
soonerbot 45:9c520ebc38e0 328 bot.smoothMove((8000.0/360.0)*(-90),1,40);
soonerbot 45:9c520ebc38e0 329 } else if(rigOnFire == 2){
soonerbot 47:bedbd8b05657 330 bot.smoothMove(290*(40-(column*16)),0,40);
soonerbot 45:9c520ebc38e0 331 bot.smoothMove(290*(3),0,40);
soonerbot 45:9c520ebc38e0 332 bot.smoothMove((8000.0/360.0)*(-90),1,40);
soonerbot 46:faad265d8ce9 333 } else if(rigOnFire == 4){
soonerbot 47:bedbd8b05657 334 bot.smoothMove(290*(68.375-(column*16)),0,40);
soonerbot 45:9c520ebc38e0 335 bot.smoothMove(290*(3),0,40);
soonerbot 45:9c520ebc38e0 336 bot.smoothMove((8000.0/360.0)*(-90),1,40);
soonerbot 45:9c520ebc38e0 337 }
soonerbot 45:9c520ebc38e0 338 break;
soonerbot 45:9c520ebc38e0 339 }
sswatek 32:ff71f61bb9f6 340 }