Diff: main.cpp

Revision:

24:46eeab7cebc6

Parent:

23:53cafcd67828

Child:

25:f5b8b0ebdcef

diff -r 53cafcd67828 -r 46eeab7cebc6 main.cpp
--- a/main.cpp	Thu Mar 13 04:30:24 2014 +0000
+++ b/main.cpp	Thu Mar 13 05:46:36 2014 +0000
@@ -52,6 +52,7 @@
     */
     Timer dataTimer;
     int testdata[]={'H','e','l','l','o'};
+    int responseData[17];
     int response;
     int gotAck=0;
     // Loop
@@ -155,6 +156,23 @@
                 dataTimer.reset();
                 while(!(gotAck=bot.BTLink.getAck(response)) && dataTimer.read_ms()<500);
                 dataTimer.stop();
+                if(0){
+                    dataTimer.reset();
+                    while(!(gotAck=bot.BTLink.getData(0x02, responseData)) && dataTimer.read_ms()<1000);
+                    if(responseData[15]==1){
+                        int squareX=(responseData[3]<<8)|responseData[2];
+                        int squareY=(responseData[5]<<8)|responseData[4];
+                        int squareRot=responseData[1];
+                        int circleX=(responseData[11]<<8)|responseData[10];
+                        int circleY=(responseData[13]<<8)|responseData[12];
+                        int triangleX=(responseData[7]<<8)|responseData[6];
+                        int triangleY=(responseData[9]<<8)|responseData[8];
+                        int triangleRot=responseData[14];
+                        DBGPRINT("Rect(%d, %d, %d) Tri(%d, %d, %d) Cir(%d, %d)",squareX,squareY,squareRot,triangleX,triangleY,triangleRot,circleX,circleY);
+                    } else {
+                        DBGPRINT("Did not pass hash check",1);
+                    }
+                }
                 //while(!bot.BTLink.getAck(response));
                 //bot.ifac*=2;
                 //DBGPRINT("ifac = %f\r\n",bot.ifac);
@@ -169,11 +187,27 @@
                 dataTimer.start();
                 dataTimer.reset();
                 while(!(gotAck=bot.BTLink.getAck(response)) && dataTimer.read_ms()<500);
+                DBGPRINT("Mode2=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
+                if(gotAck){
+                    dataTimer.reset();
+                    while(!bot.BTLink.procBuf(0x02) && !(gotAck=bot.BTLink.getData(0x02, responseData)) && dataTimer.read_ms()<1000);
+                    dataTimer.stop();
+                    if(responseData[3]==1 && gotAck){
+                        DBGPRINT("Found fire at %d in %d\n\r",responseData[2],dataTimer.read_ms());
+                    } else {
+                        DBGPRINT("Did not pass hash check %d %d %d\n\r",responseData[3],gotAck,dataTimer.read_ms());
+                    }
+                }
+                response=bot.BTLink.sendCmd(0x00,testdata,5);
+                dataTimer.start();
+                dataTimer.reset();
+                while(!(gotAck=bot.BTLink.getAck(response)) && dataTimer.read_ms()<500);
                 dataTimer.stop();
+                DBGPRINT("BackToNormal=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
+                bot.BTLink.getData(0x02, responseData);
                 //while(!bot.BTLink.getAck(response));
                 //bot.dfac*=2;
                 //DBGPRINT("dfac = %f\r\n",bot.dfac);
-                DBGPRINT("Mode2=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
                 break;
             case 'j':
                 bot.dfac/=2;