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

Committer:
sswatek
Date:
Thu Apr 03 13:17:04 2014 +0000
Revision:
49:7d172c133dbf
Parent:
48:519deb1d4dff
Child:
51:4892c9238c33
fixed bluetooth comms, servo setpoints, and developing a pickup procedure

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 49:7d172c133dbf 13 void gamePlayer::resetFirst(){
sswatek 49:7d172c133dbf 14 row = 0;
sswatek 49:7d172c133dbf 15 column = 0;
sswatek 49:7d172c133dbf 16 rigOnFire=-1;
sswatek 49:7d172c133dbf 17 waveGaps[0]=-1;
sswatek 49:7d172c133dbf 18 waveGaps[1]=-1;
sswatek 49:7d172c133dbf 19 waveGaps[2]=-1;
sswatek 49:7d172c133dbf 20 direction = -1;
sswatek 49:7d172c133dbf 21 }
sswatek 49:7d172c133dbf 22
sswatek 32:ff71f61bb9f6 23 void gamePlayer::checkWaves(){
sswatek 32:ff71f61bb9f6 24 direction = bot.gyro.getZDegrees();
sswatek 33:03b0b66038e1 25 float oldWallDist;
sswatek 33:03b0b66038e1 26 while(1){
sswatek 33:03b0b66038e1 27 oldWallDist = bot.pingLeft.getStablePoll();
sswatek 33:03b0b66038e1 28 if(oldWallDist>0.0 || oldWallDist<-1.5)
sswatek 33:03b0b66038e1 29 break;
sswatek 33:03b0b66038e1 30 }
sswatek 33:03b0b66038e1 31 DBGPRINT("at %d distance %f\r\n",column,oldWallDist);
sswatek 33:03b0b66038e1 32 float wallDist = -2;
sswatek 33:03b0b66038e1 33 if(oldWallDist > 15.0 || oldWallDist < -1.5)
sswatek 33:03b0b66038e1 34 waveGaps[row]=column;
sswatek 33:03b0b66038e1 35 float desiredHeading=0;
sswatek 33:03b0b66038e1 36 for(int i=column+1;i<6;i++){
sswatek 33:03b0b66038e1 37 //move forward one wave worth
sswatek 33:03b0b66038e1 38 bot.smoothMove(290*16,0,40);
sswatek 33:03b0b66038e1 39 //straighten back out
sswatek 33:03b0b66038e1 40 //DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 33:03b0b66038e1 41 //bot.smoothMove((8000.0/360.0)*(desiredHeading),1,40);
sswatek 33:03b0b66038e1 42 while(1){
sswatek 33:03b0b66038e1 43 wallDist = bot.pingLeft.getStablePoll();
sswatek 33:03b0b66038e1 44 if(wallDist>0.0 || wallDist<-1.5)
sswatek 33:03b0b66038e1 45 break;
sswatek 33:03b0b66038e1 46 }
sswatek 33:03b0b66038e1 47 DBGPRINT("at %d distance %f\r\n",i,wallDist);
sswatek 33:03b0b66038e1 48 if(wallDist > 15.0 || wallDist < -1.5){
sswatek 33:03b0b66038e1 49 waveGaps[row]=i;
sswatek 33:03b0b66038e1 50 DBGPRINT("See-Wall Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 34:4ad11cda1eca 51 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
sswatek 33:03b0b66038e1 52 desiredHeading = 0;
sswatek 33:03b0b66038e1 53 } else {
sswatek 33:03b0b66038e1 54 //correct our direction
sswatek 33:03b0b66038e1 55 if(!(waveGaps[row]==i-1) && i!=5){
sswatek 33:03b0b66038e1 56 float xDist = 16.0;
sswatek 33:03b0b66038e1 57 float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
sswatek 33:03b0b66038e1 58 desiredHeading = (atan2(3.9-wallDist,16.0)*180.0/3.14159)/2;
sswatek 33:03b0b66038e1 59 DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
sswatek 33:03b0b66038e1 60 DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
sswatek 34:4ad11cda1eca 61 bot.smoothMove((8000.0/360.0)*((-heading+desiredHeading)),1,40);
sswatek 33:03b0b66038e1 62 } else {
sswatek 33:03b0b66038e1 63 DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 34:4ad11cda1eca 64 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
sswatek 33:03b0b66038e1 65 desiredHeading = 0;
sswatek 33:03b0b66038e1 66 }
sswatek 33:03b0b66038e1 67 oldWallDist = wallDist;
sswatek 33:03b0b66038e1 68 }
sswatek 33:03b0b66038e1 69 }
sswatek 33:03b0b66038e1 70 if(waveGaps[row]>=0)
sswatek 33:03b0b66038e1 71 bot.smoothMove(-290*16*(5-waveGaps[row]),0,40);
sswatek 34:4ad11cda1eca 72 }
sswatek 34:4ad11cda1eca 73
sswatek 34:4ad11cda1eca 74 void gamePlayer::runFirstRow(){
sswatek 34:4ad11cda1eca 75 if(row != 0){
sswatek 34:4ad11cda1eca 76 DBGPRINT("Not in first row for runFirstRow",1);
sswatek 34:4ad11cda1eca 77 return;
sswatek 34:4ad11cda1eca 78 }
sswatek 34:4ad11cda1eca 79 direction = bot.gyro.getZDegrees();
sswatek 34:4ad11cda1eca 80 float oldWallDist;
sswatek 34:4ad11cda1eca 81 while(1){
sswatek 34:4ad11cda1eca 82 oldWallDist = bot.pingLeft.getStablePoll();
sswatek 34:4ad11cda1eca 83 if(oldWallDist>0.0 || oldWallDist<-1.5)
sswatek 34:4ad11cda1eca 84 break;
sswatek 34:4ad11cda1eca 85 }
sswatek 34:4ad11cda1eca 86 waveGaps[row]=5;
sswatek 34:4ad11cda1eca 87 DBGPRINT("at %d distance %f\r\n",column,oldWallDist);
sswatek 34:4ad11cda1eca 88 float wallDist = -2;
sswatek 34:4ad11cda1eca 89 if(oldWallDist > 15.0 || oldWallDist < -1.5)
sswatek 34:4ad11cda1eca 90 waveGaps[row]=column;
sswatek 34:4ad11cda1eca 91 float desiredHeading=0;
sswatek 34:4ad11cda1eca 92 for(int i=column+1;i<5;i++){
sswatek 34:4ad11cda1eca 93 //move forward one wave worth
sswatek 34:4ad11cda1eca 94 bot.smoothMove(290*16,0,40);
sswatek 34:4ad11cda1eca 95 //straighten back out
sswatek 34:4ad11cda1eca 96 //DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 34:4ad11cda1eca 97 //bot.smoothMove((8000.0/360.0)*(desiredHeading),1,40);
sswatek 34:4ad11cda1eca 98 while(1){
sswatek 34:4ad11cda1eca 99 wallDist = bot.pingLeft.getStablePoll();
sswatek 34:4ad11cda1eca 100 if(wallDist>0.0 || wallDist<-1.5)
sswatek 34:4ad11cda1eca 101 break;
sswatek 34:4ad11cda1eca 102 }
sswatek 34:4ad11cda1eca 103 DBGPRINT("at %d distance %f\r\n",i,wallDist);
sswatek 34:4ad11cda1eca 104 if(i==3){
sswatek 34:4ad11cda1eca 105 bot.smoothMove((8000.0/360.0)*(-desiredHeading-90),1,40);
sswatek 34:4ad11cda1eca 106 int rigFound=0;
sswatek 34:4ad11cda1eca 107 while(1){
sswatek 34:4ad11cda1eca 108 rigFound = bot.pollForRigs();
sswatek 34:4ad11cda1eca 109 if(rigFound == 1 || rigFound == 2 || rigFound == 4)
sswatek 34:4ad11cda1eca 110 break;
sswatek 34:4ad11cda1eca 111 }
sswatek 34:4ad11cda1eca 112 rigOnFire = rigFound;
sswatek 34:4ad11cda1eca 113 DBGPRINT("Found rig %d is on fire\r\n",rigFound);
sswatek 34:4ad11cda1eca 114 bot.smoothMove((8000.0/360.0)*(desiredHeading+90),1,40);
sswatek 34:4ad11cda1eca 115 }
sswatek 34:4ad11cda1eca 116 if(wallDist > 15.0 || wallDist < -1.5){
sswatek 34:4ad11cda1eca 117 waveGaps[row]=i;
sswatek 34:4ad11cda1eca 118 DBGPRINT("See-Wall Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 34:4ad11cda1eca 119 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
sswatek 34:4ad11cda1eca 120 desiredHeading = 0;
sswatek 34:4ad11cda1eca 121 } else {
sswatek 34:4ad11cda1eca 122 //correct our direction
sswatek 34:4ad11cda1eca 123 if(!(waveGaps[row]==i-1) && i!=4){
sswatek 34:4ad11cda1eca 124 float xDist = 16.0;
sswatek 34:4ad11cda1eca 125 float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
sswatek 34:4ad11cda1eca 126 desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
sswatek 34:4ad11cda1eca 127 DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
sswatek 34:4ad11cda1eca 128 DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
sswatek 34:4ad11cda1eca 129 bot.smoothMove((8000.0/360.0)*((-heading+desiredHeading)),1,40);
sswatek 34:4ad11cda1eca 130 } else {
sswatek 34:4ad11cda1eca 131 DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 34:4ad11cda1eca 132 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
sswatek 34:4ad11cda1eca 133 desiredHeading = 0;
sswatek 34:4ad11cda1eca 134 }
sswatek 34:4ad11cda1eca 135 oldWallDist = wallDist;
sswatek 34:4ad11cda1eca 136 }
sswatek 34:4ad11cda1eca 137 }
sswatek 36:67773a788d52 138 //pick up procedure goes here
sswatek 48:519deb1d4dff 139 bot.nearCamera();
sswatek 48:519deb1d4dff 140 return;
sswatek 36:67773a788d52 141 //move through gap
sswatek 34:4ad11cda1eca 142 if(waveGaps[row]>=0){
sswatek 34:4ad11cda1eca 143 bot.smoothMove(290*(16*(waveGaps[row]-4)+3),0,40);
sswatek 34:4ad11cda1eca 144 bot.smoothMove((8000.0/360.0)*(-90),1,40);
sswatek 34:4ad11cda1eca 145 if(waveGaps[row]==4){
sswatek 34:4ad11cda1eca 146 bot.smoothMove(290*(13+oldWallDist),0,40);
sswatek 34:4ad11cda1eca 147 } else {
sswatek 34:4ad11cda1eca 148 bot.smoothMove(290*(13+wallDist),0,40);
sswatek 34:4ad11cda1eca 149 }
sswatek 34:4ad11cda1eca 150 bot.smoothMove((8000.0/360.0)*(90),1,40);
sswatek 34:4ad11cda1eca 151 bot.smoothMove(290*(-3),0,40);
soonerbot 38:3062838df0ce 152 column = waveGaps[row];
sswatek 34:4ad11cda1eca 153 row = 1;
sswatek 34:4ad11cda1eca 154 }
sswatek 36:67773a788d52 155 }
sswatek 36:67773a788d52 156
sswatek 36:67773a788d52 157 void gamePlayer::runSecondRow(){
soonerbot 38:3062838df0ce 158 float gapCheck = -2;
soonerbot 45:9c520ebc38e0 159 if(row != 1 || row != 2){
soonerbot 38:3062838df0ce 160 DBGPRINT("Not in second row for runSecondRow",1);
sswatek 36:67773a788d52 161 return;
sswatek 36:67773a788d52 162 }
sswatek 36:67773a788d52 163 direction = bot.gyro.getZDegrees();
sswatek 36:67773a788d52 164 float oldWallDist;
sswatek 36:67773a788d52 165 while(1){
sswatek 36:67773a788d52 166 oldWallDist = bot.pingLeft.getStablePoll();
sswatek 36:67773a788d52 167 if(oldWallDist>0.0 || oldWallDist<-1.5)
sswatek 36:67773a788d52 168 break;
sswatek 36:67773a788d52 169 }
sswatek 36:67773a788d52 170 DBGPRINT("at %d distance %f\r\n",column,oldWallDist);
sswatek 36:67773a788d52 171 float wallDist = -2;
sswatek 36:67773a788d52 172 float desiredHeading=0;
sswatek 36:67773a788d52 173 if(oldWallDist > 15.0 || oldWallDist < -1.5) {
sswatek 36:67773a788d52 174 //found gap
sswatek 36:67773a788d52 175 waveGaps[row]=column;
soonerbot 38:3062838df0ce 176 //check if this is really the center of the gap
soonerbot 38:3062838df0ce 177 bot.smoothMove(290*4,0,40);
soonerbot 39:e61dd30febfc 178 gapCheck = bot.pingLeft.getStablePoll();
soonerbot 38:3062838df0ce 179 if((gapCheck > 15.0) || (gapCheck < -1.5)){
soonerbot 38:3062838df0ce 180 bot.smoothMove(290*-4,0,40);
soonerbot 38:3062838df0ce 181 }else{
soonerbot 38:3062838df0ce 182 bot.smoothMove(290*-6,0,40);
soonerbot 38:3062838df0ce 183 }
sswatek 36:67773a788d52 184 } else {
sswatek 36:67773a788d52 185 //check in one direction
sswatek 36:67773a788d52 186 for(int i=column+1;i<6;i++){
sswatek 36:67773a788d52 187 //move forward one wave worth
sswatek 36:67773a788d52 188 bot.smoothMove(290*16,0,40);
sswatek 36:67773a788d52 189 while(1){ //check distance to wall
sswatek 36:67773a788d52 190 wallDist = bot.pingLeft.getStablePoll();
sswatek 36:67773a788d52 191 if(wallDist>0.0 || wallDist<-1.5)
sswatek 36:67773a788d52 192 break;
sswatek 36:67773a788d52 193 }
sswatek 36:67773a788d52 194 DBGPRINT("at %d distance %f\r\n",i,wallDist);
sswatek 36:67773a788d52 195 if(wallDist > 15.0 || wallDist < -1.5){
sswatek 36:67773a788d52 196 //found gap
sswatek 36:67773a788d52 197 waveGaps[row]=i;
soonerbot 38:3062838df0ce 198 //check if this is really the center of the gap
soonerbot 38:3062838df0ce 199 bot.smoothMove(290*4,0,40);
soonerbot 39:e61dd30febfc 200 gapCheck = bot.pingLeft.getStablePoll();
soonerbot 38:3062838df0ce 201 if((gapCheck > 15.0) || (gapCheck < -1.5)){
soonerbot 38:3062838df0ce 202 bot.smoothMove(290*-4,0,40);
soonerbot 38:3062838df0ce 203 }else{
soonerbot 38:3062838df0ce 204 bot.smoothMove(290*-6,0,40);
soonerbot 38:3062838df0ce 205 }
sswatek 36:67773a788d52 206 column = i;
sswatek 36:67773a788d52 207 DBGPRINT("See-Wall Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 36:67773a788d52 208 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
sswatek 36:67773a788d52 209 desiredHeading = 0;
sswatek 36:67773a788d52 210 break;
sswatek 36:67773a788d52 211 } else {
sswatek 36:67773a788d52 212 //correct our direction
sswatek 36:67773a788d52 213 if(!(waveGaps[row]==i-1) && i==5){
sswatek 36:67773a788d52 214 //we have enough info to correct direction
sswatek 36:67773a788d52 215 // and we are as far as we will go, so need to correct for and move a long trip
sswatek 36:67773a788d52 216 float xDist = 16.0;
sswatek 36:67773a788d52 217 float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
sswatek 36:67773a788d52 218 desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
sswatek 36:67773a788d52 219 DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
sswatek 36:67773a788d52 220 DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
sswatek 36:67773a788d52 221 bot.smoothMove((8000.0/360.0)*((-heading+desiredHeading)),1,40);
sswatek 36:67773a788d52 222 } else if(!(waveGaps[row]==i-1)){
sswatek 36:67773a788d52 223 //we have enough info to correct direction
sswatek 36:67773a788d52 224 // for the move to the next stop
sswatek 36:67773a788d52 225 float xDist = 16.0;
sswatek 36:67773a788d52 226 float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
sswatek 36:67773a788d52 227 desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
sswatek 36:67773a788d52 228 DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
sswatek 36:67773a788d52 229 DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
sswatek 36:67773a788d52 230 bot.smoothMove((8000.0/360.0)*((-heading+desiredHeading)),1,40);
sswatek 36:67773a788d52 231 } else {
sswatek 36:67773a788d52 232 // not enough info
sswatek 36:67773a788d52 233 // this should never happen because we are greedy and turn the first time we see a gap
sswatek 36:67773a788d52 234 DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
sswatek 36:67773a788d52 235 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
sswatek 36:67773a788d52 236 desiredHeading = 0;
sswatek 36:67773a788d52 237 }
sswatek 36:67773a788d52 238 oldWallDist = wallDist;
sswatek 36:67773a788d52 239 }
sswatek 36:67773a788d52 240 }
soonerbot 38:3062838df0ce 241 //haven't found, check other direction
sswatek 36:67773a788d52 242 if(waveGaps[row]==-1){
soonerbot 39:e61dd30febfc 243 bot.smoothMove(290*(16*(column-5)),0,40);
soonerbot 38:3062838df0ce 244 while(1){
soonerbot 38:3062838df0ce 245 oldWallDist = bot.pingLeft.getStablePoll();
soonerbot 38:3062838df0ce 246 if(oldWallDist>0.0 || oldWallDist<-1.5)
soonerbot 38:3062838df0ce 247 break;
soonerbot 38:3062838df0ce 248 }
soonerbot 38:3062838df0ce 249 DBGPRINT("at %d distance %f\r\n",column,oldWallDist);
soonerbot 38:3062838df0ce 250
soonerbot 38:3062838df0ce 251 for(int i=column+1;i>0;i=i-1){
soonerbot 38:3062838df0ce 252 //move backward one wave worth
soonerbot 38:3062838df0ce 253 bot.smoothMove(290*-16,0,40);
soonerbot 38:3062838df0ce 254 //straighten back out
soonerbot 38:3062838df0ce 255 while(1){
soonerbot 38:3062838df0ce 256 wallDist = bot.pingLeft.getStablePoll();
soonerbot 38:3062838df0ce 257 if(wallDist>0.0 || wallDist<-1.5)
soonerbot 38:3062838df0ce 258 break;
soonerbot 38:3062838df0ce 259 }
soonerbot 38:3062838df0ce 260
soonerbot 38:3062838df0ce 261 DBGPRINT("at %d distance %f\r\n",i,wallDist);
soonerbot 38:3062838df0ce 262 if(wallDist > 15.0 || wallDist < -1.5){
soonerbot 38:3062838df0ce 263 //found gap
soonerbot 38:3062838df0ce 264 waveGaps[row]=i;
soonerbot 38:3062838df0ce 265 column = i;
soonerbot 38:3062838df0ce 266 DBGPRINT("See-Wall Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
soonerbot 38:3062838df0ce 267 bot.smoothMove((8000.0/360.0)*(-desiredHeading),1,40);
soonerbot 38:3062838df0ce 268 //check if this is really the center of the gap
soonerbot 38:3062838df0ce 269 bot.smoothMove(290*4,0,40);
soonerbot 39:e61dd30febfc 270 gapCheck = bot.pingLeft.getStablePoll();
soonerbot 38:3062838df0ce 271 if((gapCheck > 15.0) || (gapCheck < -1.5)){
soonerbot 38:3062838df0ce 272 bot.smoothMove(290*-4,0,40);
soonerbot 38:3062838df0ce 273 }else{
soonerbot 38:3062838df0ce 274 bot.smoothMove(290*-6,0,40);
soonerbot 38:3062838df0ce 275 }
soonerbot 38:3062838df0ce 276 desiredHeading = 0;
soonerbot 38:3062838df0ce 277 break;
soonerbot 45:9c520ebc38e0 278 } else {
soonerbot 38:3062838df0ce 279 //correct our direction
soonerbot 38:3062838df0ce 280 if(!(waveGaps[row]==i-1) && i==5){
soonerbot 38:3062838df0ce 281 //we have enough info to correct direction
soonerbot 38:3062838df0ce 282 // and we are as far as we will go, so need to correct for and move a long trip
soonerbot 38:3062838df0ce 283 float xDist = 16.0;
soonerbot 38:3062838df0ce 284 float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
soonerbot 38:3062838df0ce 285 desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
soonerbot 38:3062838df0ce 286 DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
soonerbot 38:3062838df0ce 287 DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
soonerbot 45:9c520ebc38e0 288 bot.smoothMove((8000.0/360.0)*(-(-heading+desiredHeading)),1,40);
soonerbot 38:3062838df0ce 289 } else if(!(waveGaps[row]==i-1)){
soonerbot 38:3062838df0ce 290 //we have enough info to correct direction
soonerbot 38:3062838df0ce 291 // for the move to the next stop
soonerbot 38:3062838df0ce 292 float xDist = 16.0;
soonerbot 38:3062838df0ce 293 float heading = atan2(wallDist-oldWallDist,xDist)*180.0/3.14159;
soonerbot 38:3062838df0ce 294 desiredHeading = (atan2(8.0-wallDist,16.0)*180.0/3.14159)/2;
soonerbot 38:3062838df0ce 295 DBGPRINT("Hd: %f, des: %f\r\n",heading,desiredHeading);
soonerbot 38:3062838df0ce 296 DBGPRINT("Turning %f degrees to fix and %f to %f\r\n",-(-heading+desiredHeading),direction,direction-heading);
soonerbot 45:9c520ebc38e0 297 bot.smoothMove((8000.0/360.0)*(-(-heading+desiredHeading)),1,40);
soonerbot 38:3062838df0ce 298 } else {
soonerbot 38:3062838df0ce 299 // not enough info
soonerbot 38:3062838df0ce 300 // this should never happen because we are greedy and turn the first time we see a gap
soonerbot 38:3062838df0ce 301 DBGPRINT("Re-Aligning %f degrees or %f\r\n",desiredHeading,(8000.0/360.0)*(desiredHeading));
soonerbot 45:9c520ebc38e0 302 bot.smoothMove((8000.0/360.0)*(desiredHeading),1,40);
soonerbot 38:3062838df0ce 303 desiredHeading = 0;
soonerbot 45:9c520ebc38e0 304 }
soonerbot 38:3062838df0ce 305 oldWallDist = wallDist;
soonerbot 38:3062838df0ce 306 }
sswatek 36:67773a788d52 307 }
sswatek 36:67773a788d52 308 }
sswatek 36:67773a788d52 309 //move through gap
sswatek 36:67773a788d52 310 if(waveGaps[row]>=0){
soonerbot 38:3062838df0ce 311 //check if this is really the center of the gap
soonerbot 38:3062838df0ce 312 bot.smoothMove(290*4,0,40);
soonerbot 39:e61dd30febfc 313 gapCheck = bot.pingLeft.getStablePoll();
soonerbot 38:3062838df0ce 314 if((gapCheck > 15.0) || (gapCheck < -1.5)){
soonerbot 38:3062838df0ce 315 bot.smoothMove(290*-4,0,40);
soonerbot 38:3062838df0ce 316 }else{
soonerbot 38:3062838df0ce 317 bot.smoothMove(290*-6,0,40);
soonerbot 38:3062838df0ce 318 }
sswatek 36:67773a788d52 319 //assumes we are already at the right column
sswatek 36:67773a788d52 320 bot.smoothMove(290*(3),0,40);
sswatek 36:67773a788d52 321 bot.smoothMove((8000.0/360.0)*(-90),1,40);
sswatek 36:67773a788d52 322 if(waveGaps[row]==4){
sswatek 36:67773a788d52 323 bot.smoothMove(290*(13+oldWallDist),0,40);
sswatek 36:67773a788d52 324 } else {
sswatek 36:67773a788d52 325 bot.smoothMove(290*(13+wallDist),0,40);
sswatek 36:67773a788d52 326 }
sswatek 36:67773a788d52 327 bot.smoothMove((8000.0/360.0)*(90),1,40);
sswatek 36:67773a788d52 328 bot.smoothMove(290*(-3),0,40);
sswatek 36:67773a788d52 329 column = waveGaps[row];
soonerbot 45:9c520ebc38e0 330 row = 2;
sswatek 36:67773a788d52 331 }
soonerbot 45:9c520ebc38e0 332 }}
soonerbot 45:9c520ebc38e0 333
soonerbot 45:9c520ebc38e0 334 void gamePlayer::approachRig(){
soonerbot 45:9c520ebc38e0 335 while(1){
soonerbot 45:9c520ebc38e0 336 if(rigOnFire == 1){
soonerbot 45:9c520ebc38e0 337 bot.smoothMove(290*11.625-(column*16),0,40);
soonerbot 45:9c520ebc38e0 338 bot.smoothMove(290*(3),0,40);
soonerbot 45:9c520ebc38e0 339 bot.smoothMove((8000.0/360.0)*(-90),1,40);
soonerbot 45:9c520ebc38e0 340 } else if(rigOnFire == 2){
soonerbot 45:9c520ebc38e0 341 bot.smoothMove(290*40-(column*16),0,40);
soonerbot 45:9c520ebc38e0 342 bot.smoothMove(290*(3),0,40);
soonerbot 45:9c520ebc38e0 343 bot.smoothMove((8000.0/360.0)*(-90),1,40);
soonerbot 45:9c520ebc38e0 344 } else if(rigOnFire == 3){
soonerbot 45:9c520ebc38e0 345 bot.smoothMove(290*68.375-(column*16),0,40);
soonerbot 45:9c520ebc38e0 346 bot.smoothMove(290*(3),0,40);
soonerbot 45:9c520ebc38e0 347 bot.smoothMove((8000.0/360.0)*(-90),1,40);
soonerbot 45:9c520ebc38e0 348 }
soonerbot 45:9c520ebc38e0 349 break;
soonerbot 45:9c520ebc38e0 350 }
sswatek 32:ff71f61bb9f6 351 }