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@34:4ad11cda1eca, 2014-03-23 (annotated)

Committer:
sswatek
Date:
Sun Mar 23 08:24:07 2014 +0000
Revision:
34:4ad11cda1eca
Parent:
33:03b0b66038e1
Child:
36:67773a788d52
Basic game logic for first row

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 34:4ad11cda1eca 128 if(waveGaps[row]>=0){
sswatek 34:4ad11cda1eca 129 bot.smoothMove(290*(16*(waveGaps[row]-4)+3),0,40);
sswatek 34:4ad11cda1eca 130 bot.smoothMove((8000.0/360.0)*(-90),1,40);
sswatek 34:4ad11cda1eca 131 if(waveGaps[row]==4){
sswatek 34:4ad11cda1eca 132 bot.smoothMove(290*(13+oldWallDist),0,40);
sswatek 34:4ad11cda1eca 133 } else {
sswatek 34:4ad11cda1eca 134 bot.smoothMove(290*(13+wallDist),0,40);
sswatek 34:4ad11cda1eca 135 }
sswatek 34:4ad11cda1eca 136 bot.smoothMove((8000.0/360.0)*(90),1,40);
sswatek 34:4ad11cda1eca 137 bot.smoothMove(290*(-3),0,40);
sswatek 34:4ad11cda1eca 138 row = 1;
sswatek 34:4ad11cda1eca 139 column = waveGaps[row];
sswatek 34:4ad11cda1eca 140 }
sswatek 32:ff71f61bb9f6 141 }