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

main.cpp@51:4892c9238c33, 2014-04-03 (annotated)

Committer:
soonerbot
Date:
Thu Apr 03 21:24:10 2014 +0000
Revision:
51:4892c9238c33
Parent:
50:ec6cc79132e8 
update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soonerbot 0:3a3dd78038a6 1 #include "mbed.h"
soonerbot 1:c28fac16a109 2 #include "dbgprint.h"
soonerbot 1:c28fac16a109 3 #include "robot.h"
soonerbot 14:a30aa3b29a2e 4 #include "hcsr04.h"
sswatek 33:03b0b66038e1 5 #include "game.h"
soonerbot 0:3a3dd78038a6 6
soonerbot 1:c28fac16a109 7 BusOut leds(LED_RED,LED_GREEN,LED_BLUE);
soonerbot 1:c28fac16a109 8 Serial pc(USBTX, USBRX);
soonerbot 0:3a3dd78038a6 9
sswatek 35:6ef5fbc45672 10
soonerbot 14:a30aa3b29a2e 11
soonerbot 14:a30aa3b29a2e 12 /*
soonerbot 14:a30aa3b29a2e 13 //actual competition code, kinda
soonerbot 14:a30aa3b29a2e 14 int firstSection(){ // drive forward and measure distance
soonerbot 14:a30aa3b29a2e 15 tmpOut = bot.absDriveForward(targetAngle,3000);
soonerbot 14:a30aa3b29a2e 16 // ping sensor
soonerbot 14:a30aa3b29a2e 17 tmpOut = bot.absDriveForward(targetAngle,3000);
soonerbot 14:a30aa3b29a2e 18 // ping sensor
soonerbot 14:a30aa3b29a2e 19 }
soonerbot 14:a30aa3b29a2e 20 int secondsection(){
soonerbot 14:a30aa3b29a2e 21 // second step
soonerbot 14:a30aa3b29a2e 22 // etc
soonerbot 14:a30aa3b29a2e 23 }
soonerbot 14:a30aa3b29a2e 24 */
soonerbot 14:a30aa3b29a2e 25
soonerbot 0:3a3dd78038a6 26 int main() {
sswatek 35:6ef5fbc45672 27 wait(0.1);
sswatek 35:6ef5fbc45672 28 robot bot;
sswatek 35:6ef5fbc45672 29 gamePlayer game(bot);
sswatek 48:519deb1d4dff 30 /*servo s(PTC9);
sswatek 37:38a4ad49343e 31 servo s2(PTC8);
sswatek 37:38a4ad49343e 32 servo s3(PTA5);
sswatek 48:519deb1d4dff 33 servo s4(PTA4);*/
soonerbot 14:a30aa3b29a2e 34 // Initialize variables
soonerbot 14:a30aa3b29a2e 35 leds = 0x2;
soonerbot 1:c28fac16a109 36 char tmpchar = 0;
soonerbot 14:a30aa3b29a2e 37 const int* constbuf; // displays values
soonerbot 10:926f142f16a3 38 int tmpOut;
soonerbot 4:adc885f4ab75 39 double targetAngle=0.0;
soonerbot 14:a30aa3b29a2e 40 DBGPRINT("AA\n\r",1); // to see if robot powers up at least
soonerbot 14:a30aa3b29a2e 41
soonerbot 15:b10859606504 42 //test servo
sswatek 48:519deb1d4dff 43 /*s.toPosition(-45);
sswatek 44:4b933e7409e7 44 s2.toPosition(60);
sswatek 48:519deb1d4dff 45 s3.toPosition(250);
sswatek 48:519deb1d4dff 46 s4.toPosition(90);*/
soonerbot 15:b10859606504 47
sswatek 42:28d73a7c624e 48 int s1pos = 45;
sswatek 42:28d73a7c624e 49 int s2pos = 45;
soonerbot 15:b10859606504 50
soonerbot 15:b10859606504 51 //timer issue navigation test
soonerbot 15:b10859606504 52 bot.absDriveForward(0,50);//assuming the bot stops in this function
sswatek 44:4b933e7409e7 53
sswatek 44:4b933e7409e7 54 bot.gyro.stop();//stopping the gyro timer
soonerbot 15:b10859606504 55 /*
soonerbot 15:b10859606504 56 DBGPRINT("%f",bot.gyro.getZDegrees());
soonerbot 15:b10859606504 57 bot.gyro.stop();//stopping the gyro timer
soonerbot 15:b10859606504 58 //bot.gyro.gyroUpkeepTicker.detach();//stopping the gyro timer
soonerbot 15:b10859606504 59 //use sensor to find distance
soonerbot 15:b10859606504 60 bot.pingLeft.trigger();
soonerbot 15:b10859606504 61 wait(0.1);
soonerbot 15:b10859606504 62 float pingresult = bot.pingLeft.inches();
soonerbot 15:b10859606504 63 DBGPRINT("Distance = %f\r\n",pingresult);
soonerbot 15:b10859606504 64 bot.gyro.start();//stopping the gyro timer
soonerbot 15:b10859606504 65 DBGPRINT("%f",bot.gyro.getZDegrees());
soonerbot 15:b10859606504 66 */
sswatek 23:53cafcd67828 67 Timer dataTimer;
sswatek 23:53cafcd67828 68 int testdata[]={'H','e','l','l','o'};
sswatek 30:4158120cf801 69 //int responseData[17];
sswatek 23:53cafcd67828 70 int response;
sswatek 23:53cafcd67828 71 int gotAck=0;
soonerbot 14:a30aa3b29a2e 72 // Loop
soonerbot 0:3a3dd78038a6 73 while(1) {
soonerbot 1:c28fac16a109 74 DBGPRINT("BB\n\r",1);
soonerbot 7:3b2cf7efe5d1 75 leds = leds^0x7; // toggle the LEDs for each loop
soonerbot 1:c28fac16a109 76 tmpchar = pc.getc();
sswatek 23:53cafcd67828 77 bot.BTLink.procBuf(0x02);
soonerbot 7:3b2cf7efe5d1 78 // all of these movement commands are blocking, so they can't be easily stopped short of resetting the microcontroller
soonerbot 1:c28fac16a109 79 switch(tmpchar){
soonerbot 15:b10859606504 80 /*case 'e': //drive in a smallish square
soonerbot 7:3b2cf7efe5d1 81 bot.driveForward(0,3000);
soonerbot 7:3b2cf7efe5d1 82 bot.driveForward(-90,0);
soonerbot 7:3b2cf7efe5d1 83 bot.driveForward(-90,3000);
soonerbot 7:3b2cf7efe5d1 84 bot.driveForward(-180,0);
soonerbot 7:3b2cf7efe5d1 85 bot.driveForward(-180,3000);
soonerbot 7:3b2cf7efe5d1 86 bot.driveForward(-270,0);
soonerbot 7:3b2cf7efe5d1 87 bot.driveForward(-270,3000);
soonerbot 7:3b2cf7efe5d1 88 bot.driveForward(0,0);
soonerbot 7:3b2cf7efe5d1 89 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 15:b10859606504 90 break;*/
soonerbot 7:3b2cf7efe5d1 91 case 'q': //poll the encoders
soonerbot 1:c28fac16a109 92 constbuf = bot.bigenc.getVals();
soonerbot 1:c28fac16a109 93 DBGPRINT("\n\r%d\t%d\t%d\t%d\n\r",constbuf[0],constbuf[1],constbuf[2],constbuf[3]);
soonerbot 1:c28fac16a109 94 break;
soonerbot 7:3b2cf7efe5d1 95 case 'z': //set the current direction to "forward" for the following "go forward/reverse" commands
soonerbot 4:adc885f4ab75 96 targetAngle = bot.gyro.getZDegrees();
soonerbot 4:adc885f4ab75 97 break;
sswatek 49:7d172c133dbf 98 case 'w':
sswatek 49:7d172c133dbf 99 DBGPRINT("100 Forward\n\r",1);
sswatek 49:7d172c133dbf 100 bot.absDriveForward(0,100);
sswatek 49:7d172c133dbf 101 // run first row
soonerbot 15:b10859606504 102
sswatek 49:7d172c133dbf 103 /*DBGPRINT("Running First Row\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
sswatek 49:7d172c133dbf 104 game.resetFirst();
sswatek 49:7d172c133dbf 105 game.runFirstRow();*/
soonerbot 7:3b2cf7efe5d1 106 break;
soonerbot 7:3b2cf7efe5d1 107 case 'x': // turn 90 degrees clockwise from the starting rotation
sswatek 48:519deb1d4dff 108 //s.toPosition(220);
soonerbot 15:b10859606504 109 //bot.driveForward(-90,0);
soonerbot 6:62d498ee97cf 110 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 6:62d498ee97cf 111 break;
soonerbot 7:3b2cf7efe5d1 112 case 'a': // turn the opposite direction from the starting one
sswatek 48:519deb1d4dff 113 //s.toPosition(90);
soonerbot 15:b10859606504 114 //bot.driveForward(180,0);
sswatek 49:7d172c133dbf 115 //DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
sswatek 49:7d172c133dbf 116
sswatek 49:7d172c133dbf 117 DBGPRINT("100 Left\n\r",1);
sswatek 49:7d172c133dbf 118 bot.smoothMove(-100, 1, 40);
soonerbot 1:c28fac16a109 119 break;
soonerbot 7:3b2cf7efe5d1 120 case 'd': // turn back to starting rotation
soonerbot 15:b10859606504 121 //bot.driveForward(0,0);
sswatek 49:7d172c133dbf 122 //response=bot.BTLink.sendCmd(0x02,testdata,5);
sswatek 49:7d172c133dbf 123 //DBGPRINT("=%d\n\r",response);
sswatek 49:7d172c133dbf 124 DBGPRINT("100 Right\n\r",1);
sswatek 49:7d172c133dbf 125 bot.smoothMove(100, 1, 40);
soonerbot 7:3b2cf7efe5d1 126 break;
soonerbot 7:3b2cf7efe5d1 127 case 'W': // drive forward a small bit
soonerbot 9:aff48e331147 128 //bot.driveForward(targetAngle,1000);
soonerbot 10:926f142f16a3 129 //bot.motors.moveForward(100);
soonerbot 11:967469d7e01c 130 tmpOut = bot.absDriveForward(targetAngle,3000);
soonerbot 10:926f142f16a3 131 DBGPRINT("W -> %d\n\r",tmpOut);
soonerbot 12:925f52da3ba9 132 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 4:adc885f4ab75 133 break;
soonerbot 7:3b2cf7efe5d1 134 case 'E': //drive forward five times as much
soonerbot 15:b10859606504 135 //bot.driveForward(targetAngle,5000);
sswatek 31:4ef53fbd6759 136 bot.absDriveForward(0,290*16);
soonerbot 4:adc885f4ab75 137 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 4:adc885f4ab75 138 break;
soonerbot 7:3b2cf7efe5d1 139 case 'X': // small reverse
soonerbot 10:926f142f16a3 140 //bot.driveForward(targetAngle,-1000);
soonerbot 10:926f142f16a3 141
soonerbot 11:967469d7e01c 142 tmpOut = bot.absDriveForward(targetAngle,-3000);
soonerbot 10:926f142f16a3 143 DBGPRINT("X -> %d\n\r",tmpOut);
soonerbot 12:925f52da3ba9 144 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 4:adc885f4ab75 145 break;
soonerbot 7:3b2cf7efe5d1 146 case 'C': // big reverse
soonerbot 15:b10859606504 147 //bot.driveForward(targetAngle,-5000);
sswatek 31:4ef53fbd6759 148 bot.absDriveForward(0,-290*16);
soonerbot 4:adc885f4ab75 149 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 4:adc885f4ab75 150 break;
soonerbot 7:3b2cf7efe5d1 151 case 'p': // poll the gyro
soonerbot 3:a223b0bf8256 152 DBGPRINT("%d\r\n",bot.gyro.getZ());
soonerbot 3:a223b0bf8256 153 break;
soonerbot 7:3b2cf7efe5d1 154 case 'l': //calibrate the gyro ( caution, only use after resetting and before moving, otherwise will break any calibration)
soonerbot 3:a223b0bf8256 155 bot.gyro.calibrate();
soonerbot 3:a223b0bf8256 156 break;
soonerbot 7:3b2cf7efe5d1 157
soonerbot 7:3b2cf7efe5d1 158 // these are all control system modifications which should be fairly well locked down at this point
soonerbot 4:adc885f4ab75 159 case 't':
sswatek 18:c834bd26869e 160 response=bot.BTLink.sendCmd(0x00,testdata,5);
sswatek 23:53cafcd67828 161 //bot.gyro.stop();
sswatek 23:53cafcd67828 162 dataTimer.start();
sswatek 23:53cafcd67828 163 dataTimer.reset();
sswatek 30:4158120cf801 164 while(1){
sswatek 30:4158120cf801 165 gotAck=bot.BTLink.getAck(response);
sswatek 30:4158120cf801 166 if(gotAck || dataTimer.read_ms()>=500)
sswatek 30:4158120cf801 167 break;
sswatek 30:4158120cf801 168 }
sswatek 23:53cafcd67828 169 dataTimer.stop();
sswatek 23:53cafcd67828 170 //bot.gyro.start();
sswatek 18:c834bd26869e 171 //bot.pfac*=2;
sswatek 18:c834bd26869e 172 //DBGPRINT("pfac = %f\r\n",bot.pfac);
sswatek 23:53cafcd67828 173 DBGPRINT("Mode0=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
soonerbot 4:adc885f4ab75 174 break;
soonerbot 4:adc885f4ab75 175 case 'g':
sswatek 29:1132155bc7da 176 DBGPRINT("full rotation %d,%f -> ",bot.gyro.getZ(),bot.gyro.getZDegrees());
sswatek 33:03b0b66038e1 177 for(int i=0;i<36;i++){
sswatek 33:03b0b66038e1 178 bot.smoothMove((8000.0/360.0)*(10.0), 1, 40);
sswatek 33:03b0b66038e1 179 }
sswatek 29:1132155bc7da 180 DBGPRINT(" %d,%f\r\n",bot.gyro.getZ(),bot.gyro.getZDegrees());
soonerbot 4:adc885f4ab75 181 break;
soonerbot 4:adc885f4ab75 182 case 'y':
sswatek 26:ade7c813538f 183 bot.pollForShapes();
sswatek 50:ec6cc79132e8 184 DBGPRINT("dist %f\r\n",(480.0-bot.triY)/75.0 + 1.65);
soonerbot 4:adc885f4ab75 185 break;
soonerbot 4:adc885f4ab75 186 case 'h':
sswatek 29:1132155bc7da 187 DBGPRINT("full rotation %d,%f -> ",bot.gyro.getZ(),bot.gyro.getZDegrees());
sswatek 29:1132155bc7da 188 bot.smoothMove(-8000, 1, 40);
sswatek 29:1132155bc7da 189 DBGPRINT(" %d,%f\r\n",bot.gyro.getZ(),bot.gyro.getZDegrees());
soonerbot 4:adc885f4ab75 190 break;
soonerbot 4:adc885f4ab75 191 case 'u':
sswatek 27:688409727452 192 bot.pollForRigs();
soonerbot 4:adc885f4ab75 193 break;
soonerbot 4:adc885f4ab75 194 case 'j':
sswatek 31:4ef53fbd6759 195 DBGPRINT("LeftStable (15,8) = %f\r\n",bot.pingLeft.getStablePoll());
soonerbot 4:adc885f4ab75 196 break;
soonerbot 4:adc885f4ab75 197 case 'i':
soonerbot 14:a30aa3b29a2e 198 //put distance ping here
soonerbot 14:a30aa3b29a2e 199 bot.pingLeft.trigger();
sswatek 30:4158120cf801 200 dataTimer.start();
sswatek 30:4158120cf801 201 dataTimer.reset();
sswatek 30:4158120cf801 202 while(bot.pingLeft.measuring && dataTimer.read_ms()<300);
sswatek 30:4158120cf801 203 dataTimer.stop();
sswatek 30:4158120cf801 204 DBGPRINT("Distance = %f\r\n",bot.pingLeft.inches());
soonerbot 4:adc885f4ab75 205 break;
soonerbot 4:adc885f4ab75 206 case 'k':
sswatek 30:4158120cf801 207 dataTimer.start();
sswatek 30:4158120cf801 208 for(int i=0;i<100;i++){
sswatek 30:4158120cf801 209 bot.pingLeft.trigger();
sswatek 30:4158120cf801 210 dataTimer.reset();
sswatek 30:4158120cf801 211 while(bot.pingLeft.measuring && dataTimer.read_ms()<300);
sswatek 30:4158120cf801 212 DBGPRINT("Distance %d = %f\r\n",i,bot.pingLeft.inches());
sswatek 30:4158120cf801 213 }
sswatek 30:4158120cf801 214 dataTimer.stop();
sswatek 26:ade7c813538f 215 //bot.angfac/=2;
sswatek 26:ade7c813538f 216 //DBGPRINT("angfac = %f\r\n",bot.angfac);
soonerbot 4:adc885f4ab75 217 break;
soonerbot 4:adc885f4ab75 218 case 'T':
sswatek 48:519deb1d4dff 219 DBGPRINT("Grabbing Tool\r\n",1);
sswatek 48:519deb1d4dff 220 //s.toPosition(-45);
sswatek 48:519deb1d4dff 221 //s2.toPosition(60);
sswatek 48:519deb1d4dff 222 bot.grabPosition();
sswatek 44:4b933e7409e7 223 wait(0.5);
sswatek 48:519deb1d4dff 224 bot.grab();
sswatek 48:519deb1d4dff 225 /*s2.toPosition(-10);
sswatek 44:4b933e7409e7 226 wait(0.5);
sswatek 44:4b933e7409e7 227 //bot.cont.start();
sswatek 44:4b933e7409e7 228 bot.cont.resetTicks();
sswatek 44:4b933e7409e7 229 bot.cont.rampSpeed(-20,-20);
sswatek 44:4b933e7409e7 230 //s.toPosition(0);
sswatek 44:4b933e7409e7 231 //wait(.5);
sswatek 44:4b933e7409e7 232 while(bot.cont.avgTicks()>-900){
sswatek 44:4b933e7409e7 233 s.toPosition((-45)+((35+45)*(-bot.cont.avgTicks()))/900);
sswatek 44:4b933e7409e7 234 wait_ms(20);
sswatek 44:4b933e7409e7 235 }
sswatek 44:4b933e7409e7 236 bot.cont.rampSpeed(0,0);
sswatek 44:4b933e7409e7 237 s.toPosition(130);
sswatek 48:519deb1d4dff 238 while(!bot.cont.stopped());*/
sswatek 44:4b933e7409e7 239 //bot.cont.stop();
sswatek 44:4b933e7409e7 240 //game.checkWaves();
soonerbot 4:adc885f4ab75 241 break;
soonerbot 4:adc885f4ab75 242 case 'G':
soonerbot 38:3062838df0ce 243 DBGPRINT("Running First and Second Row\r\n",1);
sswatek 34:4ad11cda1eca 244 game.runFirstRow();
soonerbot 38:3062838df0ce 245 game.runSecondRow();
soonerbot 45:9c520ebc38e0 246 game.runSecondRow();
soonerbot 45:9c520ebc38e0 247 game.approachRig();
sswatek 26:ade7c813538f 248 //bot.pfac/=1.05;
sswatek 26:ade7c813538f 249 //DBGPRINT("pfac = %f\r\n",bot.pfac);
soonerbot 4:adc885f4ab75 250 break;
soonerbot 45:9c520ebc38e0 251 case 's':
sswatek 49:7d172c133dbf 252 DBGPRINT("100 Back\n\r",1);
sswatek 49:7d172c133dbf 253 bot.smoothMove(-100, 0, 40);
sswatek 49:7d172c133dbf 254 //game.runSecondRow(); //testing second and third row
sswatek 49:7d172c133dbf 255 //game.runSecondRow();
soonerbot 45:9c520ebc38e0 256 break;
soonerbot 51:4892c9238c33 257 case 'S':
soonerbot 51:4892c9238c33 258 game.approachRig(); //testing
soonerbot 51:4892c9238c33 259 break;
soonerbot 4:adc885f4ab75 260 case 'Y':
sswatek 44:4b933e7409e7 261 /*bot.cont.start();
sswatek 43:f7ef0f5f980c 262 bot.cont.rampSpeed(40,40);
sswatek 44:4b933e7409e7 263 while(!bot.cont.steady());
sswatek 43:f7ef0f5f980c 264 bot.cont.rampSpeed(0,0);
sswatek 44:4b933e7409e7 265 while(!bot.cont.stopped());
sswatek 44:4b933e7409e7 266 bot.cont.stop();*/
sswatek 48:519deb1d4dff 267 //s3.toPosition(s1pos+=5);
sswatek 48:519deb1d4dff 268 //bot.alignWithRig();
sswatek 49:7d172c133dbf 269
sswatek 49:7d172c133dbf 270 bot.grabPosition();
sswatek 42:28d73a7c624e 271 DBGPRINT("1 pos = %d\r\n",s1pos);
soonerbot 4:adc885f4ab75 272 break;
soonerbot 4:adc885f4ab75 273 case 'H':
sswatek 49:7d172c133dbf 274 DBGPRINT("2 pos = %d\r\n",bot.incServo());
sswatek 48:519deb1d4dff 275 //s3.toPosition(s1pos-=5);
sswatek 49:7d172c133dbf 276 //DBGPRINT("1 pos = %d\r\n",s1pos);
soonerbot 4:adc885f4ab75 277 break;
soonerbot 4:adc885f4ab75 278 case 'U':
sswatek 48:519deb1d4dff 279 //s4.toPosition(s2pos+=5);
sswatek 49:7d172c133dbf 280 bot.extendStick();
sswatek 42:28d73a7c624e 281 DBGPRINT("2 pos = %d\r\n",s2pos);
soonerbot 4:adc885f4ab75 282 break;
soonerbot 4:adc885f4ab75 283 case 'J':
sswatek 48:519deb1d4dff 284 //s4.toPosition(s2pos-=5);
sswatek 49:7d172c133dbf 285 DBGPRINT("2 pos = %d\r\n",bot.decServo());
sswatek 49:7d172c133dbf 286 //DBGPRINT("2 pos = %d\r\n",s2pos);
soonerbot 4:adc885f4ab75 287 break;
soonerbot 7:3b2cf7efe5d1 288
soonerbot 7:3b2cf7efe5d1 289 // poll the compass ( currently gives bad values )
soonerbot 4:adc885f4ab75 290 case 'Q':
sswatek 50:ec6cc79132e8 291 bot.nearCamera();
sswatek 50:ec6cc79132e8 292 wait(1.5);
sswatek 50:ec6cc79132e8 293 bot.extendStick();
sswatek 50:ec6cc79132e8 294 wait(0.75);
sswatek 50:ec6cc79132e8 295 bot.downCamera();
sswatek 50:ec6cc79132e8 296 wait(1.5);
sswatek 50:ec6cc79132e8 297 bot.pollForShapes();
sswatek 50:ec6cc79132e8 298 if(bot.triX != 0){
sswatek 50:ec6cc79132e8 299 //found triangle
sswatek 50:ec6cc79132e8 300 //float dist = (9.75*(480.0-bot.triY))/480.0 + 0.5;
sswatek 50:ec6cc79132e8 301 float dist = (480.0-bot.triY)/75.0 + 1.65;
sswatek 50:ec6cc79132e8 302 bot.smoothMove(290*(dist-6.0), 0, 5);
sswatek 50:ec6cc79132e8 303 bot.smoothMove(-600, 1, 5); // turn left
sswatek 50:ec6cc79132e8 304 bot.smoothMove(150, 1, 5); // turn right
sswatek 50:ec6cc79132e8 305 bot.smoothMove(290*(-3.5), 0, 5);
sswatek 50:ec6cc79132e8 306 bot.smoothMove(-350, 1, 5); // turn left
sswatek 50:ec6cc79132e8 307 bot.smoothMove(350, 1, 5); // turn back right
sswatek 50:ec6cc79132e8 308 bot.nearCamera();
sswatek 50:ec6cc79132e8 309 wait(1.0);
sswatek 50:ec6cc79132e8 310 bot.retractStick();
sswatek 50:ec6cc79132e8 311 wait(1.0);
sswatek 50:ec6cc79132e8 312 bot.downCamera();
sswatek 50:ec6cc79132e8 313 wait(1.5);
sswatek 50:ec6cc79132e8 314 bot.smoothMove(290*(4.0), 0, 5);
sswatek 50:ec6cc79132e8 315 bot.pollForShapes();
sswatek 50:ec6cc79132e8 316 if(bot.triX != 0 && bot.triRot > 52 && bot.triRot < 67){
sswatek 50:ec6cc79132e8 317 if(bot.triX > 362 || bot.triX < 337){
sswatek 50:ec6cc79132e8 318 int correction = (100*(bot.triX - 349))/60;
sswatek 50:ec6cc79132e8 319 bot.smoothMove(correction, 1, 5); // turn rightish to correct
sswatek 50:ec6cc79132e8 320 wait(0.5);
sswatek 50:ec6cc79132e8 321 bot.pollForShapes();
sswatek 50:ec6cc79132e8 322
sswatek 50:ec6cc79132e8 323 //right is positive and decreases
sswatek 50:ec6cc79132e8 324 }
sswatek 50:ec6cc79132e8 325 //dist = (9.75*(480.0-bot.triY))/480.0 + 0.5;
sswatek 50:ec6cc79132e8 326 dist = (480.0-bot.triY)/75.0 + 1.65;
sswatek 50:ec6cc79132e8 327 bot.smoothMove(290*(dist), 0, 5);
sswatek 50:ec6cc79132e8 328 bot.grab();
sswatek 50:ec6cc79132e8 329 }
sswatek 50:ec6cc79132e8 330 }
sswatek 50:ec6cc79132e8 331 /*
sswatek 49:7d172c133dbf 332 bot.downCamera();
sswatek 49:7d172c133dbf 333 wait(3);
sswatek 49:7d172c133dbf 334 bot.pollForShapes();
sswatek 49:7d172c133dbf 335 if(bot.triX != 0){
sswatek 49:7d172c133dbf 336 //found triangle
sswatek 49:7d172c133dbf 337 float dist = (8.25*(480.0-bot.triY))/480.0 + 1.0;
sswatek 49:7d172c133dbf 338 if(!(bot.triRot > 52 && bot.triRot < 67)){
sswatek 49:7d172c133dbf 339 bot.extendStick();
sswatek 49:7d172c133dbf 340 bot.smoothMove(-1000, 1, 5); // turn left
sswatek 49:7d172c133dbf 341 bot.retractStick();
sswatek 49:7d172c133dbf 342 bot.smoothMove(50, 1, 5); // turn right
sswatek 49:7d172c133dbf 343 if(dist>4.0)
sswatek 49:7d172c133dbf 344 bot.smoothMove(290*(dist-5), 0, 5);
sswatek 49:7d172c133dbf 345 bot.smoothMove(425, 1, 5); // turn left
sswatek 49:7d172c133dbf 346 wait(0.5);
sswatek 49:7d172c133dbf 347 bot.pollForShapes();
sswatek 49:7d172c133dbf 348 }
sswatek 49:7d172c133dbf 349 if(bot.triX != 0 && bot.triRot > 52 && bot.triRot < 67){
sswatek 49:7d172c133dbf 350 if(bot.triX > 375 || bot.triX < 360){
sswatek 49:7d172c133dbf 351 int correction = (100*(bot.triX - 367))/65;
sswatek 49:7d172c133dbf 352 bot.smoothMove(correction, 1, 5); // turn rightish to correct
sswatek 49:7d172c133dbf 353 wait(0.5);
sswatek 49:7d172c133dbf 354 bot.pollForShapes();
sswatek 49:7d172c133dbf 355
sswatek 49:7d172c133dbf 356 //right is positive and decreases
sswatek 49:7d172c133dbf 357 }
sswatek 49:7d172c133dbf 358 dist = (9.6*(480.0-bot.triY))/480.0 + 0.9;
sswatek 49:7d172c133dbf 359 bot.smoothMove(290*(dist-0.2), 0, 5);
sswatek 49:7d172c133dbf 360 bot.grab();
sswatek 49:7d172c133dbf 361 }
sswatek 49:7d172c133dbf 362
sswatek 49:7d172c133dbf 363 }
sswatek 50:ec6cc79132e8 364 */
sswatek 28:c7e8d2db03f5 365 //bot.gyro.checkCompass();
sswatek 49:7d172c133dbf 366 //DBGPRINT("Compass vector = %d\t%d\t%d\t%f\r\n",bot.gyro.xmag,bot.gyro.ymag,bot.gyro.zmag,bot.gyro.compDir*180.0/3.14159);
soonerbot 4:adc885f4ab75 367 break;
soonerbot 7:3b2cf7efe5d1 368
soonerbot 7:3b2cf7efe5d1 369 // brake if we get an unknown command
soonerbot 1:c28fac16a109 370 default:
soonerbot 15:b10859606504 371 //bot.left.brake();
soonerbot 15:b10859606504 372 //bot.right.brake();
soonerbot 1:c28fac16a109 373 break;
soonerbot 1:c28fac16a109 374
soonerbot 1:c28fac16a109 375 }
soonerbot 0:3a3dd78038a6 376 }
soonerbot 0:3a3dd78038a6 377 }