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@25:f5b8b0ebdcef, 2014-03-13 (annotated)

Committer:
sswatek
Date:
Thu Mar 13 06:25:31 2014 +0000
Revision:
25:f5b8b0ebdcef
Parent:
24:46eeab7cebc6
Child:
26:ade7c813538f
recieves shape data with 'y' except for triangle rotation

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"
soonerbot 0:3a3dd78038a6 5
soonerbot 1:c28fac16a109 6 BusOut leds(LED_RED,LED_GREEN,LED_BLUE);
soonerbot 1:c28fac16a109 7 Serial pc(USBTX, USBRX);
soonerbot 0:3a3dd78038a6 8
soonerbot 1:c28fac16a109 9 robot bot;
soonerbot 15:b10859606504 10 servo s(PTC9);
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() {
soonerbot 14:a30aa3b29a2e 27 // Initialize variables
soonerbot 14:a30aa3b29a2e 28 leds = 0x2;
soonerbot 1:c28fac16a109 29 char tmpchar = 0;
soonerbot 14:a30aa3b29a2e 30 const int* constbuf; // displays values
soonerbot 10:926f142f16a3 31 int tmpOut;
soonerbot 4:adc885f4ab75 32 double targetAngle=0.0;
soonerbot 14:a30aa3b29a2e 33 DBGPRINT("AA\n\r",1); // to see if robot powers up at least
soonerbot 14:a30aa3b29a2e 34
soonerbot 15:b10859606504 35 //test servo
soonerbot 15:b10859606504 36 s.toPosition(45);
soonerbot 15:b10859606504 37
soonerbot 15:b10859606504 38
soonerbot 15:b10859606504 39 //timer issue navigation test
soonerbot 15:b10859606504 40 bot.absDriveForward(0,50);//assuming the bot stops in this function
soonerbot 15:b10859606504 41 /*
soonerbot 15:b10859606504 42 DBGPRINT("%f",bot.gyro.getZDegrees());
soonerbot 15:b10859606504 43 bot.gyro.stop();//stopping the gyro timer
soonerbot 15:b10859606504 44 //bot.gyro.gyroUpkeepTicker.detach();//stopping the gyro timer
soonerbot 15:b10859606504 45 //use sensor to find distance
soonerbot 15:b10859606504 46 bot.pingLeft.trigger();
soonerbot 15:b10859606504 47 wait(0.1);
soonerbot 15:b10859606504 48 float pingresult = bot.pingLeft.inches();
soonerbot 15:b10859606504 49 DBGPRINT("Distance = %f\r\n",pingresult);
soonerbot 15:b10859606504 50 bot.gyro.start();//stopping the gyro timer
soonerbot 15:b10859606504 51 DBGPRINT("%f",bot.gyro.getZDegrees());
soonerbot 15:b10859606504 52 */
sswatek 23:53cafcd67828 53 Timer dataTimer;
sswatek 23:53cafcd67828 54 int testdata[]={'H','e','l','l','o'};
sswatek 24:46eeab7cebc6 55 int responseData[17];
sswatek 23:53cafcd67828 56 int response;
sswatek 23:53cafcd67828 57 int gotAck=0;
soonerbot 14:a30aa3b29a2e 58 // Loop
soonerbot 0:3a3dd78038a6 59 while(1) {
soonerbot 1:c28fac16a109 60 DBGPRINT("BB\n\r",1);
soonerbot 7:3b2cf7efe5d1 61 leds = leds^0x7; // toggle the LEDs for each loop
soonerbot 1:c28fac16a109 62 tmpchar = pc.getc();
sswatek 23:53cafcd67828 63 bot.BTLink.procBuf(0x02);
soonerbot 7:3b2cf7efe5d1 64 // all of these movement commands are blocking, so they can't be easily stopped short of resetting the microcontroller
soonerbot 1:c28fac16a109 65 switch(tmpchar){
soonerbot 15:b10859606504 66 /*case 'e': //drive in a smallish square
soonerbot 7:3b2cf7efe5d1 67 bot.driveForward(0,3000);
soonerbot 7:3b2cf7efe5d1 68 bot.driveForward(-90,0);
soonerbot 7:3b2cf7efe5d1 69 bot.driveForward(-90,3000);
soonerbot 7:3b2cf7efe5d1 70 bot.driveForward(-180,0);
soonerbot 7:3b2cf7efe5d1 71 bot.driveForward(-180,3000);
soonerbot 7:3b2cf7efe5d1 72 bot.driveForward(-270,0);
soonerbot 7:3b2cf7efe5d1 73 bot.driveForward(-270,3000);
soonerbot 7:3b2cf7efe5d1 74 bot.driveForward(0,0);
soonerbot 7:3b2cf7efe5d1 75 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 15:b10859606504 76 break;*/
soonerbot 7:3b2cf7efe5d1 77 case 'q': //poll the encoders
soonerbot 1:c28fac16a109 78 constbuf = bot.bigenc.getVals();
soonerbot 1:c28fac16a109 79 DBGPRINT("\n\r%d\t%d\t%d\t%d\n\r",constbuf[0],constbuf[1],constbuf[2],constbuf[3]);
soonerbot 1:c28fac16a109 80 break;
soonerbot 7:3b2cf7efe5d1 81 case 'z': //set the current direction to "forward" for the following "go forward/reverse" commands
soonerbot 4:adc885f4ab75 82 targetAngle = bot.gyro.getZDegrees();
soonerbot 4:adc885f4ab75 83 break;
soonerbot 7:3b2cf7efe5d1 84 case 'w': // turn 90 degrees counter clockwise from the starting rotation
soonerbot 15:b10859606504 85
soonerbot 15:b10859606504 86 s.toPosition(0);
soonerbot 15:b10859606504 87 //bot.driveForward(90,0);
soonerbot 7:3b2cf7efe5d1 88 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 7:3b2cf7efe5d1 89 break;
soonerbot 7:3b2cf7efe5d1 90 case 'x': // turn 90 degrees clockwise from the starting rotation
soonerbot 15:b10859606504 91 s.toPosition(180);
soonerbot 15:b10859606504 92 //bot.driveForward(-90,0);
soonerbot 6:62d498ee97cf 93 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 6:62d498ee97cf 94 break;
soonerbot 7:3b2cf7efe5d1 95 case 'a': // turn the opposite direction from the starting one
soonerbot 15:b10859606504 96 s.toPosition(90);
soonerbot 15:b10859606504 97 //bot.driveForward(180,0);
soonerbot 6:62d498ee97cf 98 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 1:c28fac16a109 99 break;
soonerbot 7:3b2cf7efe5d1 100 case 'd': // turn back to starting rotation
soonerbot 15:b10859606504 101 //bot.driveForward(0,0);
sswatek 18:c834bd26869e 102 response=bot.BTLink.sendCmd(0x02,testdata,5);
sswatek 17:e247d58d9f42 103 DBGPRINT("=%d\n\r",response);
soonerbot 7:3b2cf7efe5d1 104 break;
soonerbot 7:3b2cf7efe5d1 105 case 'W': // drive forward a small bit
soonerbot 9:aff48e331147 106 //bot.driveForward(targetAngle,1000);
soonerbot 10:926f142f16a3 107 //bot.motors.moveForward(100);
soonerbot 11:967469d7e01c 108 tmpOut = bot.absDriveForward(targetAngle,3000);
soonerbot 10:926f142f16a3 109 DBGPRINT("W -> %d\n\r",tmpOut);
soonerbot 12:925f52da3ba9 110 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 4:adc885f4ab75 111 break;
soonerbot 7:3b2cf7efe5d1 112 case 'E': //drive forward five times as much
soonerbot 15:b10859606504 113 //bot.driveForward(targetAngle,5000);
soonerbot 15:b10859606504 114 bot.absDriveForward(0,5000);
soonerbot 4:adc885f4ab75 115 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 4:adc885f4ab75 116 break;
soonerbot 7:3b2cf7efe5d1 117 case 'X': // small reverse
soonerbot 10:926f142f16a3 118 //bot.driveForward(targetAngle,-1000);
soonerbot 10:926f142f16a3 119
soonerbot 11:967469d7e01c 120 tmpOut = bot.absDriveForward(targetAngle,-3000);
soonerbot 10:926f142f16a3 121 DBGPRINT("X -> %d\n\r",tmpOut);
soonerbot 12:925f52da3ba9 122 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 4:adc885f4ab75 123 break;
soonerbot 7:3b2cf7efe5d1 124 case 'C': // big reverse
soonerbot 15:b10859606504 125 //bot.driveForward(targetAngle,-5000);
soonerbot 15:b10859606504 126 bot.absDriveForward(0,-5000);
soonerbot 4:adc885f4ab75 127 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 4:adc885f4ab75 128 break;
soonerbot 7:3b2cf7efe5d1 129 case 'p': // poll the gyro
soonerbot 3:a223b0bf8256 130 DBGPRINT("%d\r\n",bot.gyro.getZ());
soonerbot 3:a223b0bf8256 131 break;
soonerbot 7:3b2cf7efe5d1 132 case 'l': //calibrate the gyro ( caution, only use after resetting and before moving, otherwise will break any calibration)
soonerbot 3:a223b0bf8256 133 bot.gyro.calibrate();
soonerbot 3:a223b0bf8256 134 break;
soonerbot 7:3b2cf7efe5d1 135
soonerbot 7:3b2cf7efe5d1 136 // these are all control system modifications which should be fairly well locked down at this point
soonerbot 4:adc885f4ab75 137 case 't':
sswatek 18:c834bd26869e 138 response=bot.BTLink.sendCmd(0x00,testdata,5);
sswatek 23:53cafcd67828 139 //bot.gyro.stop();
sswatek 23:53cafcd67828 140 dataTimer.start();
sswatek 23:53cafcd67828 141 dataTimer.reset();
sswatek 23:53cafcd67828 142 while(!(gotAck=bot.BTLink.getAck(response)) && dataTimer.read_ms()<500);
sswatek 23:53cafcd67828 143 dataTimer.stop();
sswatek 23:53cafcd67828 144 //bot.gyro.start();
sswatek 18:c834bd26869e 145 //bot.pfac*=2;
sswatek 18:c834bd26869e 146 //DBGPRINT("pfac = %f\r\n",bot.pfac);
sswatek 23:53cafcd67828 147 DBGPRINT("Mode0=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
soonerbot 4:adc885f4ab75 148 break;
soonerbot 4:adc885f4ab75 149 case 'g':
soonerbot 4:adc885f4ab75 150 bot.pfac/=2;
soonerbot 4:adc885f4ab75 151 DBGPRINT("pfac = %f\r\n",bot.pfac);
soonerbot 4:adc885f4ab75 152 break;
soonerbot 4:adc885f4ab75 153 case 'y':
sswatek 18:c834bd26869e 154 response=bot.BTLink.sendCmd(0x01,testdata,5);
sswatek 23:53cafcd67828 155 dataTimer.start();
sswatek 23:53cafcd67828 156 dataTimer.reset();
sswatek 23:53cafcd67828 157 while(!(gotAck=bot.BTLink.getAck(response)) && dataTimer.read_ms()<500);
sswatek 25:f5b8b0ebdcef 158 DBGPRINT("Mode1=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
sswatek 25:f5b8b0ebdcef 159 if(gotAck){
sswatek 24:46eeab7cebc6 160 dataTimer.reset();
sswatek 25:f5b8b0ebdcef 161 while(!bot.BTLink.procBuf(0x01) && !(gotAck=bot.BTLink.getData(0x01, responseData)) && dataTimer.read_ms()<4000);
sswatek 24:46eeab7cebc6 162 if(responseData[15]==1){
sswatek 25:f5b8b0ebdcef 163 for(int i=0;i<16;i++)
sswatek 25:f5b8b0ebdcef 164 DBGPRINT("%d,",responseData[i]);
sswatek 25:f5b8b0ebdcef 165 DBGPRINT("\n\r",1);
sswatek 25:f5b8b0ebdcef 166 int squareX=(responseData[4]<<8)|responseData[3];
sswatek 25:f5b8b0ebdcef 167 int squareY=(responseData[6]<<8)|responseData[5];
sswatek 25:f5b8b0ebdcef 168 int squareRot=responseData[2];
sswatek 25:f5b8b0ebdcef 169 int circleX=(responseData[12]<<8)|responseData[11];
sswatek 25:f5b8b0ebdcef 170 int circleY=(responseData[14]<<8)|responseData[13];
sswatek 25:f5b8b0ebdcef 171 int triangleX=(responseData[8]<<8)|responseData[7];
sswatek 25:f5b8b0ebdcef 172 int triangleY=(responseData[10]<<8)|responseData[9];
sswatek 25:f5b8b0ebdcef 173 int triangleRot=responseData[15];
sswatek 25:f5b8b0ebdcef 174 DBGPRINT("Rect(%d, %d, %d) Tri(%d, %d, %d) Cir(%d, %d)\n\r",squareX,squareY,squareRot,triangleX,triangleY,triangleRot,circleX,circleY);
sswatek 24:46eeab7cebc6 175 } else {
sswatek 25:f5b8b0ebdcef 176 DBGPRINT("Did not pass hash check %d %d %d\n\r",responseData[15],gotAck,dataTimer.read_ms());
sswatek 24:46eeab7cebc6 177 }
sswatek 24:46eeab7cebc6 178 }
sswatek 25:f5b8b0ebdcef 179 response=bot.BTLink.sendCmd(0x00,testdata,5);
sswatek 25:f5b8b0ebdcef 180 dataTimer.start();
sswatek 25:f5b8b0ebdcef 181 dataTimer.reset();
sswatek 25:f5b8b0ebdcef 182 while(!(gotAck=bot.BTLink.getAck(response)) && dataTimer.read_ms()<500);
sswatek 25:f5b8b0ebdcef 183 dataTimer.stop();
sswatek 25:f5b8b0ebdcef 184 DBGPRINT("BackToNormal=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
sswatek 23:53cafcd67828 185 //while(!bot.BTLink.getAck(response));
sswatek 18:c834bd26869e 186 //bot.ifac*=2;
sswatek 18:c834bd26869e 187 //DBGPRINT("ifac = %f\r\n",bot.ifac);
soonerbot 4:adc885f4ab75 188 break;
soonerbot 4:adc885f4ab75 189 case 'h':
soonerbot 4:adc885f4ab75 190 bot.ifac/=2;
soonerbot 4:adc885f4ab75 191 DBGPRINT("ifac = %f\r\n",bot.ifac);
soonerbot 4:adc885f4ab75 192 break;
soonerbot 4:adc885f4ab75 193 case 'u':
sswatek 18:c834bd26869e 194 response=bot.BTLink.sendCmd(0x02,testdata,5);
sswatek 23:53cafcd67828 195 dataTimer.start();
sswatek 23:53cafcd67828 196 dataTimer.reset();
sswatek 23:53cafcd67828 197 while(!(gotAck=bot.BTLink.getAck(response)) && dataTimer.read_ms()<500);
sswatek 24:46eeab7cebc6 198 DBGPRINT("Mode2=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
sswatek 24:46eeab7cebc6 199 if(gotAck){
sswatek 24:46eeab7cebc6 200 dataTimer.reset();
sswatek 24:46eeab7cebc6 201 while(!bot.BTLink.procBuf(0x02) && !(gotAck=bot.BTLink.getData(0x02, responseData)) && dataTimer.read_ms()<1000);
sswatek 24:46eeab7cebc6 202 dataTimer.stop();
sswatek 24:46eeab7cebc6 203 if(responseData[3]==1 && gotAck){
sswatek 24:46eeab7cebc6 204 DBGPRINT("Found fire at %d in %d\n\r",responseData[2],dataTimer.read_ms());
sswatek 24:46eeab7cebc6 205 } else {
sswatek 24:46eeab7cebc6 206 DBGPRINT("Did not pass hash check %d %d %d\n\r",responseData[3],gotAck,dataTimer.read_ms());
sswatek 24:46eeab7cebc6 207 }
sswatek 24:46eeab7cebc6 208 }
sswatek 24:46eeab7cebc6 209 response=bot.BTLink.sendCmd(0x00,testdata,5);
sswatek 24:46eeab7cebc6 210 dataTimer.start();
sswatek 24:46eeab7cebc6 211 dataTimer.reset();
sswatek 24:46eeab7cebc6 212 while(!(gotAck=bot.BTLink.getAck(response)) && dataTimer.read_ms()<500);
sswatek 23:53cafcd67828 213 dataTimer.stop();
sswatek 24:46eeab7cebc6 214 DBGPRINT("BackToNormal=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
sswatek 25:f5b8b0ebdcef 215 //bot.BTLink.getData(0x02, responseData);
sswatek 23:53cafcd67828 216 //while(!bot.BTLink.getAck(response));
sswatek 18:c834bd26869e 217 //bot.dfac*=2;
sswatek 18:c834bd26869e 218 //DBGPRINT("dfac = %f\r\n",bot.dfac);
soonerbot 4:adc885f4ab75 219 break;
soonerbot 4:adc885f4ab75 220 case 'j':
soonerbot 4:adc885f4ab75 221 bot.dfac/=2;
soonerbot 4:adc885f4ab75 222 DBGPRINT("dfac = %f\r\n",bot.dfac);
soonerbot 4:adc885f4ab75 223 break;
soonerbot 4:adc885f4ab75 224 case 'i':
soonerbot 14:a30aa3b29a2e 225 //put distance ping here
soonerbot 14:a30aa3b29a2e 226 bot.pingLeft.trigger();
soonerbot 15:b10859606504 227 wait(0.1);
soonerbot 14:a30aa3b29a2e 228 float pingresult = bot.pingLeft.inches();
soonerbot 14:a30aa3b29a2e 229 DBGPRINT("Distance = %f\r\n",pingresult);
soonerbot 4:adc885f4ab75 230 break;
soonerbot 4:adc885f4ab75 231 case 'k':
soonerbot 4:adc885f4ab75 232 bot.angfac/=2;
soonerbot 4:adc885f4ab75 233 DBGPRINT("angfac = %f\r\n",bot.angfac);
soonerbot 4:adc885f4ab75 234 break;
soonerbot 4:adc885f4ab75 235 case 'T':
soonerbot 4:adc885f4ab75 236 bot.pfac*=1.05;
soonerbot 4:adc885f4ab75 237 DBGPRINT("pfac = %f\r\n",bot.pfac);
soonerbot 4:adc885f4ab75 238 break;
soonerbot 4:adc885f4ab75 239 case 'G':
soonerbot 4:adc885f4ab75 240 bot.pfac/=1.05;
soonerbot 4:adc885f4ab75 241 DBGPRINT("pfac = %f\r\n",bot.pfac);
soonerbot 4:adc885f4ab75 242 break;
soonerbot 4:adc885f4ab75 243 case 'Y':
soonerbot 4:adc885f4ab75 244 bot.ifac*=1.05;
soonerbot 4:adc885f4ab75 245 DBGPRINT("ifac = %f\r\n",bot.ifac);
soonerbot 4:adc885f4ab75 246 break;
soonerbot 4:adc885f4ab75 247 case 'H':
soonerbot 4:adc885f4ab75 248 bot.ifac/=1.05;
soonerbot 4:adc885f4ab75 249 DBGPRINT("ifac = %f\r\n",bot.ifac);
soonerbot 4:adc885f4ab75 250 break;
soonerbot 4:adc885f4ab75 251 case 'U':
soonerbot 4:adc885f4ab75 252 bot.dfac*=1.05;
soonerbot 4:adc885f4ab75 253 DBGPRINT("dfac = %f\r\n",bot.dfac);
soonerbot 4:adc885f4ab75 254 break;
soonerbot 4:adc885f4ab75 255 case 'J':
soonerbot 4:adc885f4ab75 256 bot.dfac/=1.05;
soonerbot 4:adc885f4ab75 257 DBGPRINT("dfac = %f\r\n",bot.dfac);
soonerbot 4:adc885f4ab75 258 break;
soonerbot 7:3b2cf7efe5d1 259
soonerbot 7:3b2cf7efe5d1 260 // poll the compass ( currently gives bad values )
soonerbot 4:adc885f4ab75 261 case 'Q':
soonerbot 4:adc885f4ab75 262 DBGPRINT("Compass vector = %d\t%d\t%d\r\n",bot.gyro.xmag,bot.gyro.ymag,bot.gyro.zmag);
soonerbot 4:adc885f4ab75 263 break;
soonerbot 7:3b2cf7efe5d1 264
soonerbot 7:3b2cf7efe5d1 265 // brake if we get an unknown command
soonerbot 1:c28fac16a109 266 default:
soonerbot 15:b10859606504 267 //bot.left.brake();
soonerbot 15:b10859606504 268 //bot.right.brake();
soonerbot 1:c28fac16a109 269 break;
soonerbot 1:c28fac16a109 270
soonerbot 1:c28fac16a109 271 }
soonerbot 0:3a3dd78038a6 272 }
soonerbot 0:3a3dd78038a6 273 }