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@48:519deb1d4dff, 2014-04-03 (annotated)

Committer:
sswatek
Date:
Thu Apr 03 06:17:32 2014 +0000
Revision:
48:519deb1d4dff
Parent:
45:9c520ebc38e0
Child:
49:7d172c133dbf
Moved servos to be in robot class and added convenience functions for them

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