Diff: main_ps3.cpp

Revision:

2:738b28f6a04b

Parent:

1:3ac2087996f3

Child:

3:8d8c25c556ae

--- a/main_ps3.cpp	Wed Oct 28 09:26:31 2015 +0000
+++ b/main_ps3.cpp	Fri Oct 30 09:21:03 2015 +0000
@@ -10,7 +10,7 @@
 #define IM920
 //#define PS3
 
-/******/
+/***マシン状態計測***/
 //#define MESURE
 
 #if defined(IM920) && defined(PS3)
@@ -27,10 +27,10 @@
 
 Serial pc(USBTX, USBRX);
 #ifdef MESURE
+short mesureflag=0;
 LocalFileSystem local("local");
 #endif
 
-short cal=0;
 Ticker Com;
 void Call(){
     mesure_state();
@@ -41,10 +41,16 @@
             targ_velocity=spcount;
         }
     }
+#ifdef MESURE
+    if(mesureflag){
+        fprintf(fp_r, "%1.3f %f\r\n",time,velocity);
+        time+=0.01;
+    }
+#endif
 }
 
 int main() {
-    Com.attach(&Call,0.01);
+    Com.attach(&Call,RATE);
 #ifdef IM920
     initializeIM920();
 #else
@@ -84,6 +90,8 @@
             }
             if((step==1)&&(x>8600.0+deff)) {
                 targ_velocity=0.0;
+                direction_controller.setBias(0.0);
+                direction_controller.reset();
                 step=2;
             }
             if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
@@ -133,6 +141,8 @@
             }
             if((step==6)&&(x>7000.0)){
                 targ_velocity=0.0;
+                direction_controller.reset();
+                direction_controller.setBias(0.0);
                 step=7;
             }
             if((step==7)&&((velocity<500.0)&&(velocity>-500.0))){
@@ -177,6 +187,8 @@
                 spcount=0.0;
                 targ_sita=0.035;
                 step=18;
+                direction_controller.reset();
+                direction_controller.setBias(0.0);
             }
             if((step==18)&&(x<-100.0)){
                 targ_velocity=0.0;
@@ -209,6 +221,9 @@
             }
             if((step==1)&&(x>8600.0+deff)) {
                 targ_velocity=0.0;
+                direction_controller.reset();
+                direction_controller.setBias(0.0);
+                direction_controller.reset();
                 step=2;
             }
             if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
@@ -260,6 +275,8 @@
             }
             if((step==6)&&(x>7000.0)){
                 targ_velocity=0.0;
+                direction_controller.reset();
+                direction_controller.setBias(0.0);
                 step=7;
             }
             if((step==7)&&((velocity<500.0)&&(velocity>-500.0))){
@@ -306,6 +323,8 @@
                 spcount=0.0;
                 targ_sita=-0.035;
                 step=18;
+                direction_controller.reset();
+                direction_controller.setBias(0.0);
             }
             if((step==18)&&(x<-100.0)){
                 targ_velocity=0.0;
@@ -330,6 +349,10 @@
                 
             }
 #endif
+#ifdef MESURE
+            if(down) fclose(fp_r);
+            if(b==9) fclose(fp_r);
+#endif
 //            mesure_state();   /*位置測定*/
 //            move_following(); /*移動制御*/
         }
@@ -342,13 +365,6 @@
             manualMovePS3();   /*analogStick*/
             manualPS3();       /*PS3 button*/
 #endif
-#ifdef MESURE
-            if(down){
-                IndicatorAuto=0;
-                fclose(fp_r);
-            }
-            time+=0.01;
-#endif
         }
         /***update state***/ 
 #ifdef IM920