Diff: main_ps3.cpp

Revision:

21:bdf8ac5c200c

Parent:

19:ed178d04e11d

Child:

23:26f9483439fe

--- a/main_ps3.cpp	Thu Nov 05 08:21:23 2015 +0000
+++ b/main_ps3.cpp	Fri Nov 06 11:35:39 2015 +0000
@@ -13,15 +13,24 @@
 #error Caution, You should define either BLUE or RED
 #endif
 
+#ifdef MESURE
+#include "mbed.h"
+short mesureflag=0;
+LocalFileSystem local("local")
+short testflag=0;
+FILE *fp_r = fopen("/local/velocity.dat", "w");
+#endif
+#include "mbed.h"
+LocalFileSystem local("local");
+FILE *fp_r = fopen("/local/velocity.dat", "w");
+
 #include "machine_ps3.h"
 
 #define deff 70.0
 
 Serial pc(USBTX, USBRX);
-#ifdef MESURE
-short mesureflag=0;
-LocalFileSystem local("local");
-#endif
+
+
 
 int main() {
     Com.attach(&Call,RATE);
@@ -38,21 +47,18 @@
 #endif
     Indicator4=1;
     Enable=1;
-#ifdef MESURE
-    FILE *fp_r = fopen("/local/velocity.dat", "w");
-    double time=0.0;
-#endif
     wait(0.3);
     sendData(7,0);
     while(1) {
         if(autoflag){
             autoIM920(); /*IM920 button*/
 #ifdef BLUE
+            Indicator2=1;
             //Blue            
             /********************************Nomal Mode*********************************/
             if((step==0)&&((9000.0>x)&&(x>800.0))) {
-//                targ_sita=-0.01;
-                targ_sita=0.0;
+                targ_sita=-0.05;
+//                targ_sita=0.0;
                 step=1;
             }
 //            if((step==1)&&(x>8600.0+deff)) {
@@ -82,7 +88,15 @@
             }
             /***Cylinder***/
             if((x>3130.0+deff)&&(CStep==1)) { 
-                if(!skip) sendData(1,1);
+//            if((x>2030.0+deff)&&(CStep==1)) { 
+//                if(!skip){ sendData(1,1);
+                if(!skip){
+                    sendData(1,5);
+                    fprintf(fp_r, "x:%f\r\n",x);
+#ifdef MESURE
+                    testflag=1;
+#endif
+                }
                 CStep=2; 
             }
             if((x>5900.0+deff)&&(CStep==2)) {
@@ -319,9 +333,9 @@
             }
 #endif
 #ifdef MESURE
-            if(down) fclose(fp_r);
             if(b==9) fclose(fp_r);
 #endif
+            if(b==9) fclose(fp_r);
         }
         else if(!autoflag) {
             flaga=0;
@@ -330,8 +344,8 @@
             manualIM920();     /*IM920 button*/
         }
         /***update state***/ 
-//        pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
-        pc.printf("a2:%d ,b:%d ,X:%d ,Y:%d\r\n",a2,b,X,Y);
+        pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
+//        pc.printf("a2:%d ,b:%d ,X:%d ,Y:%d\r\n",a2,b,X,Y);
 //        wait(RATE);
     }
 }
\ No newline at end of file