Diff: main.cpp

Revision:

25:f5b8b0ebdcef

Parent:

24:46eeab7cebc6

Child:

26:ade7c813538f

--- a/main.cpp	Thu Mar 13 05:46:36 2014 +0000
+++ b/main.cpp	Thu Mar 13 06:25:31 2014 +0000
@@ -155,28 +155,36 @@
                 dataTimer.start();
                 dataTimer.reset();
                 while(!(gotAck=bot.BTLink.getAck(response)) && dataTimer.read_ms()<500);
-                dataTimer.stop();
-                if(0){
+                DBGPRINT("Mode1=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
+                if(gotAck){
                     dataTimer.reset();
-                    while(!(gotAck=bot.BTLink.getData(0x02, responseData)) && dataTimer.read_ms()<1000);
+                    while(!bot.BTLink.procBuf(0x01) && !(gotAck=bot.BTLink.getData(0x01, responseData)) && dataTimer.read_ms()<4000);
                     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);
+                        for(int i=0;i<16;i++)
+                            DBGPRINT("%d,",responseData[i]);
+                        DBGPRINT("\n\r",1);
+                        int squareX=(responseData[4]<<8)|responseData[3];
+                        int squareY=(responseData[6]<<8)|responseData[5];
+                        int squareRot=responseData[2];
+                        int circleX=(responseData[12]<<8)|responseData[11];
+                        int circleY=(responseData[14]<<8)|responseData[13];
+                        int triangleX=(responseData[8]<<8)|responseData[7];
+                        int triangleY=(responseData[10]<<8)|responseData[9];
+                        int triangleRot=responseData[15];
+                        DBGPRINT("Rect(%d, %d, %d) Tri(%d, %d, %d) Cir(%d, %d)\n\r",squareX,squareY,squareRot,triangleX,triangleY,triangleRot,circleX,circleY);
                     } else {
-                        DBGPRINT("Did not pass hash check",1);
+                        DBGPRINT("Did not pass hash check %d %d %d\n\r",responseData[15],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());
                 //while(!bot.BTLink.getAck(response));
                 //bot.ifac*=2;
                 //DBGPRINT("ifac = %f\r\n",bot.ifac);
-                DBGPRINT("Mode1=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
                 break;
             case 'h':
                 bot.ifac/=2;
@@ -204,7 +212,7 @@
                 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);
+                //bot.BTLink.getData(0x02, responseData);
                 //while(!bot.BTLink.getAck(response));
                 //bot.dfac*=2;
                 //DBGPRINT("dfac = %f\r\n",bot.dfac);