Diff: main_ps3.cpp

Revision:

71:4ebc1c0fcb4e

Parent:

70:5b8ed76a31c7

Child:

72:021a14ee970f

--- a/main_ps3.cpp	Fri Oct 09 07:20:12 2015 +0000
+++ b/main_ps3.cpp	Fri Oct 09 12:01:58 2015 +0000
@@ -5,11 +5,12 @@
 //速度コントローラと向きコントローラはそのまま
 //大和田にPIC1のシリンダの信号をタイマーで書かせる
 //回転機構wait PI/4 追加で待つ
-//射出をスキップする機能を追加
+//射出をスキップする機能を追加(サイクリックでCstepを変える)
+//スタートゾーンにマシンを戻した後に暴走しないようにする
 
 /***コース選択***/
-#define BLUE
-//#define RED
+//#define BLUE
+#define RED
 
 /***コントローラ選択***/
 //#define IM920
@@ -77,25 +78,25 @@
             }
             if((step==3)&&(x<70.0)) targ_velocity=0.0,step=8;*/
             
-            if((step==4)&&((5700.0>x)&&(x>900.0))) targ_sita=0.03,step=5; /*middle*/
+            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<1750.0)) {
+            if((step==6)&&(x<1600.0)) {
                 targ_sita=PI/4;
                 step=7;
                 sendData(7,31);
             }
-            if((step==7)&&(x<70.0)) targ_velocity=0.0,step=8;
+            if((step==7)&&(x<-100.0)) targ_velocity=0.0,step=8;
             
-            if((x>2900.0)&&(CStep==1)) CStep=2,sendData(1,1); /*nomal*/
-            if((x>5850.0)&&(CStep==2)) CStep=3,sendData(1,2);
-            if((x>7600.0)&&(CStep==3)) CStep=4,sendData(1,3);
-            if((x<6600.0)&&(CStep==4)) CStep=5,sendData(1,5);
-            if((x<6100.0)&&(CStep==5)) CStep=6,sendData(1,4);
+            if((x>2900.0)&&(CStep==1)&&(!skip)) CStep=2,sendData(1,1); /*nomal*/
+            if((x>5850.0)&&(CStep==2)&&(!skip)) CStep=3,sendData(1,2);
+            if((x>7600.0)&&(CStep==3)&&(!skip)) CStep=4,sendData(1,3);
+            if((x<6600.0)&&(CStep==4)&&(!skip)) CStep=5,sendData(1,5);
+            if((x<6100.0)&&(CStep==5)&&(!skip)) CStep=6,sendData(1,4);
             
             /*if((x>3344.0)&&(CStep==1)) CStep=2,sendData(1,1);
             if((x>6234.0)&&(CStep==2)) CStep=3,sendData(1,2);
@@ -103,27 +104,37 @@
             if((x<6750.0)&&(CStep==4)) CStep=5,sendData(1,5);
             if((x<6300.0)&&(CStep==5)) CStep=6,sendData(1,4);*/
 
-#else
+#else       
             //Red
-            if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=-0.015,step=1;
-            if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2;
-            if((step==2)&&(x<2000.0)) targ_sita=-PI/4,step=3;
-            if((step==3)&&(x<10.0)) targ_velocity=0.0,step=8;
+            if((step==0)&&((8650.0>x)&&(x>900.0))) targ_sita=-0.03,step=1;
+            if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2,flagf=0;
+            if((step==2)&&(x<1700.0)) {
+                targ_sita=-PI/4;
+                step=3;
+                sendData(7,31);
+            }
+            if((step==3)&&(x<-100.0)) targ_velocity=0.0,step=8;
             
-            if((step==4)&&((5700.0>x)&&(x>950.0))) targ_sita=-0.015,step=5; /*middle*/
-            if((step==5)&&(x>5300.0)) { /*middle shoot*/
+            
+            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;
-            if((step==7)&&(x<10.0)) targ_velocity=0.0,step=8;
+            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;
             
-            if((x>3344.0)&&(CStep==1)) CStep=2,sendData(1,1);
-            if((x>6234.0)&&(CStep==2)) CStep=3,sendData(1,3);
-            if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,2);
-            if((x<6700.0)&&(CStep==4)) CStep=5,sendData(1,4);
-            if((x<6300.0)&&(CStep==5)) CStep=6,sendData(1,5);
+            if((x>2900.0)&&(CStep==1)&&(!skip)) CStep=2,sendData(1,1);
+            if((x>5850.0)&&(CStep==2)&&(!skip)) CStep=3,sendData(1,3);
+            if((x>7600.0)&&(CStep==3)&&(!skip)) CStep=4,sendData(1,2);
+            if((x<6600.0)&&(CStep==4)&&(!skip)) CStep=5,sendData(1,4);
+            if((x<6100.0)&&(CStep==5)&&(!skip)) CStep=6,sendData(1,5);
 #endif
             move_following();
         }