Diff: main_ps3.cpp

Revision:

76:f9bcb7ac438f

Parent:

75:4cdcd50ee666

Child:

77:7dd154b5ace3

--- a/main_ps3.cpp	Sat Oct 10 02:40:41 2015 +0000
+++ b/main_ps3.cpp	Sat Oct 10 04:28:27 2015 +0000
@@ -5,7 +5,7 @@
 //速度コントローラと向きコントローラはそのまま
 //大和田にPIC1のシリンダの信号をタイマーで書かせる
 //回転機構wait PI/4 追加で待つ
-//射出をスキップする機能を追加(サイクリックでCstepを変える)
+//射出をスキップする機能を追加
 //スタートゾーンにマシンを戻した後に暴走しないようにする
 //手動時の操作性の改善
 //一度手動モードになったらスタートゾーンに戻るまで再び自動モードにならないようにする
@@ -63,6 +63,7 @@
 #endif
 #ifdef BLUE
             //Blue            
+            /********************************Nomal Mode*********************************/
             if((step==0)&&((8650.0>x)&&(x>900.0))) targ_sita=0.03,step=1; /*nomal*/
             if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2,flagf=0;
             if((step==2)&&(x<1700.0)) {
@@ -72,28 +73,10 @@
             }
             if((step==3)&&(x<-100.0)) {
                 targ_velocity=0.0;
-                step=8;
+                step=114;
                 autoflag = 0;
             }
-            
-            if((step==4)&&((5700.0>x)&&(x>600.0))) targ_sita=0.03,step=5; /*middle*/
-            if((step==5)&&(x>5000.0)) { /*middle shoot*/
-                targ_velocity=-speed;
-                step=6;
-                sendData(1,5);
-                flagf=0;
-            }
-            if((step==6)&&(x<1600.0)) {
-                targ_sita=PI/4;
-                step=7;
-                sendData(7,31);
-            }
-            if((step==7)&&(x<-100.0)) {
-                targ_velocity=0.0;
-                step=8;
-                autoflag = 0;
-            }
-            
+            /***Cylinder***/
             if((x>2900.0)&&(CStep==1)) { /*nomal*/
                 if(!skip) sendData(1,1);
                 CStep=2;
@@ -114,10 +97,30 @@
                 if(!skip) sendData(1,4);
                 CStep=6;
             }
-
+            /////////////////////////////////////////////////////////////////////////
+            /******************************Middle Mode******************************/
+            if((step==4)&&((5700.0>x)&&(x>600.0))) targ_sita=0.03,step=5; /*middle*/
+            if((step==5)&&(x>5000.0)) { 
+                targ_velocity=-speed;
+                step=6;
+                sendData(1,5);
+                flagf=0;
+            }
+            if((step==6)&&(x<1600.0)) {
+                targ_sita=PI/4;
+                step=7;
+                sendData(7,31);
+            }
+            if((step==7)&&(x<-100.0)) {
+                targ_velocity=0.0;
+                step=114;
+                autoflag = 0;
+            }
+            /////////////////////////////////////////////////////////////////////////
 #else       
             //Red
-            if((step==0)&&((8650.0>x)&&(x>900.0))) targ_sita=-0.03,step=1;
+            /********************************Nomal Mode*********************************/
+            if((step==0)&&((8650.0>x)&&(x>900.0))) targ_sita=-0.03,step=1; /*nomal*/
             if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2,flagf=0;
             if((step==2)&&(x<1700.0)) {
                 targ_sita=-PI/4;
@@ -129,26 +132,8 @@
                 step=8;
                 autoflag=0;
             }
-            
-            if((step==4)&&((5700.0>x)&&(x>600.0))) targ_sita=-0.03,step=5; /*middle*/
-            if((step==5)&&(x>5000.0)) { /*middle shoot*/
-                targ_velocity=-speed;
-                step=6;
-                sendData(1,4);
-                flagf=0;
-            }
-            if((step==6)&&(x<2000.0)) {
-                targ_sita=-PI/4;
-                step=7;
-                sendData(7,31);
-            }
-            if((step==7)&&(x<-100.0)) {
-                targ_velocity=0.0;
-                step=8;
-                autoflag=0;
-            }
-            
-            if((x>2900.0)&&(CStep==1)) {
+            /***Cylinder***/
+            if((x>2900.0)&&(CStep==1)) { /*nomal*/
                 if(!skip) sendData(1,1);
                 CStep=2;
             }
@@ -168,9 +153,29 @@
                 if(!skip) sendData(1,5);
                 CStep=6;
             }
+            /////////////////////////////////////////////////////////////////////////
+            /******************************Middle Mode******************************/
+            if((step==4)&&((5700.0>x)&&(x>600.0))) targ_sita=-0.03,step=5; /*middle*/
+            if((step==5)&&(x>5000.0)) {
+                targ_velocity=-speed;
+                step=6;
+                sendData(1,4);
+                flagf=0;
+            }
+            if((step==6)&&(x<2000.0)) {
+                targ_sita=-PI/4;
+                step=7;
+                sendData(7,31);
+            }
+            if((step==7)&&(x<-100.0)) {
+                targ_velocity=0.0;
+                step=8;
+                autoflag=0;
+            }
+            /////////////////////////////////////////////////////////////////////////
 #endif
             mesure_state();   /*位置測定*/
-            move_following(); /*位置移動*/
+            move_following(); /*移動制御*/
         }
         else if(!autoflag) {
             resetState();