Diff: main.cpp

Revision:

23:53cafcd67828

Parent:

18:c834bd26869e

Child:

24:46eeab7cebc6

--- a/main.cpp	Wed Mar 12 22:34:37 2014 +0000
+++ b/main.cpp	Thu Mar 13 04:30:24 2014 +0000
@@ -50,14 +50,16 @@
     bot.gyro.start();//stopping the gyro timer
     DBGPRINT("%f",bot.gyro.getZDegrees());
     */
-    
+    Timer dataTimer;
+    int testdata[]={'H','e','l','l','o'};
+    int response;
+    int gotAck=0;
     // Loop
     while(1) {
         DBGPRINT("BB\n\r",1);
         leds = leds^0x7; // toggle the LEDs for each loop
         tmpchar = pc.getc();
-        int testdata[]={'H','e','l','l','o'};
-        int response;
+        bot.BTLink.procBuf(0x02);
         // all of these movement commands are blocking, so they can't be easily stopped short of resetting the microcontroller
         switch(tmpchar){
             /*case 'e': //drive in a smallish square
@@ -133,9 +135,15 @@
                 // these are all control system modifications which should be fairly well locked down at this point
             case 't':
                 response=bot.BTLink.sendCmd(0x00,testdata,5);
+                //bot.gyro.stop();
+                dataTimer.start();
+                dataTimer.reset();
+                while(!(gotAck=bot.BTLink.getAck(response)) && dataTimer.read_ms()<500);
+                dataTimer.stop();
+                //bot.gyro.start();
                 //bot.pfac*=2;
                 //DBGPRINT("pfac = %f\r\n",bot.pfac);
-                DBGPRINT("=%d, %d\n\r",response,bot.BTLink.bufSize());
+                DBGPRINT("Mode0=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
                 break;
             case 'g':
                 bot.pfac/=2;
@@ -143,9 +151,14 @@
                 break;
             case 'y':
                 response=bot.BTLink.sendCmd(0x01,testdata,5);
+                dataTimer.start();
+                dataTimer.reset();
+                while(!(gotAck=bot.BTLink.getAck(response)) && dataTimer.read_ms()<500);
+                dataTimer.stop();
+                //while(!bot.BTLink.getAck(response));
                 //bot.ifac*=2;
                 //DBGPRINT("ifac = %f\r\n",bot.ifac);
-                DBGPRINT("=%d, %d\n\r",response,bot.BTLink.bufSize());
+                DBGPRINT("Mode1=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
                 break;
             case 'h':
                 bot.ifac/=2;
@@ -153,9 +166,14 @@
                 break;
             case 'u':
                 response=bot.BTLink.sendCmd(0x02,testdata,5);
+                dataTimer.start();
+                dataTimer.reset();
+                while(!(gotAck=bot.BTLink.getAck(response)) && dataTimer.read_ms()<500);
+                dataTimer.stop();
+                //while(!bot.BTLink.getAck(response));
                 //bot.dfac*=2;
                 //DBGPRINT("dfac = %f\r\n",bot.dfac);
-                DBGPRINT("=%d, %d\n\r",response,bot.BTLink.bufSize());
+                DBGPRINT("Mode2=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
                 break;
             case 'j':
                 bot.dfac/=2;