Diff: main.cpp

Revision:

29:1132155bc7da

Parent:

28:c7e8d2db03f5

Child:

30:4158120cf801

--- a/main.cpp	Fri Mar 21 00:48:29 2014 +0000
+++ b/main.cpp	Fri Mar 21 03:16:15 2014 +0000
@@ -147,78 +147,20 @@
                 DBGPRINT("Mode0=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
                 break;
             case 'g':
-                //bot.pfac/=2;
-                //DBGPRINT("pfac = %f\r\n",bot.pfac);
+                DBGPRINT("full rotation %d,%f -> ",bot.gyro.getZ(),bot.gyro.getZDegrees());
+                bot.smoothMove(8000, 1, 40);
+                DBGPRINT(" %d,%f\r\n",bot.gyro.getZ(),bot.gyro.getZDegrees());
                 break;
             case 'y':
                 bot.pollForShapes();
-                /*response=bot.BTLink.sendCmd(0x01,testdata,5);
-                dataTimer.start();
-                dataTimer.reset();
-                while(!(gotAck=bot.BTLink.getAck(response)) && dataTimer.read_ms()<500);
-                DBGPRINT("Mode1=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
-                if(gotAck){
-                    dataTimer.reset();
-                    while(!bot.BTLink.procBuf(0x01) && !(gotAck=bot.BTLink.getData(0x01, responseData)) && dataTimer.read_ms()<4000);
-                    if(responseData[15]==1){
-                        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 %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);
                 break;
             case 'h':
-                //bot.ifac/=2;
-                //DBGPRINT("ifac = %f\r\n",bot.ifac);
+                DBGPRINT("full rotation %d,%f -> ",bot.gyro.getZ(),bot.gyro.getZDegrees());
+                bot.smoothMove(-8000, 1, 40);
+                DBGPRINT(" %d,%f\r\n",bot.gyro.getZ(),bot.gyro.getZDegrees());
                 break;
             case 'u':
                 bot.pollForRigs();
-                /*response=bot.BTLink.sendCmd(0x02,testdata,5);
-                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);
                 break;
             case 'j':
                 //bot.dfac/=2;