Diff: main_ps3.cpp

Revision:

62:02a44f5bc51e

Parent:

61:e018207ae860

Child:

64:a98fe602c26d

--- a/main_ps3.cpp	Sat Oct 03 00:52:21 2015 +0000
+++ b/main_ps3.cpp	Sun Oct 04 07:18:36 2015 +0000
@@ -5,6 +5,8 @@
 //速度コントローラと向きコントローラはそのまま
 //変更前のこのプログラムで実験
 //後で関数のinline宣言を外す
+//大和田に最後にPIC1から信号を送り射角を変えるプログラムをに書かせる
+//大和田にPIC1のシリンダの信号をタイマーで書かせる
 
 /***コース選択***/
 #define BLUE
@@ -47,6 +49,7 @@
     Indicator4=1;
     Enable=1;
 //    FILE *fp_r = fopen("/local/velocity.dat", "w");
+    wait(0.3);
     sendData(4,31);
     wait(0.1);
     sendData(5,31);
@@ -67,11 +70,13 @@
             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==4)&&((5700.0>x)&&(x>700.0))) targ_sita=0.015,step=5; /*middle*/
-            if((step==5)&&(x>5300.0)) { /*middle shoot*/
+            if((step==4)&&((5700.0>x)&&(x>1000.0))) targ_sita=0.015,step=5; /*middle*/
+//            if((step==5)&&(x>5300.0)) { /*middle shoot*/
+            if((step==5)&&(x>6250.0)) { /*middle shoot*/
                 targ_velocity=-speed;
                 step=6;
-                sendData(1,5);
+                sendData(1,4);
+//                sendData(1,5);
             }
             if((step==6)&&(x<2000.0)) targ_sita=PI/4,step=7;
             if((step==7)&&(x<10.0)) targ_velocity=0.0,step=8;
@@ -81,14 +86,24 @@
             if((x>6234.0)&&(CStep==2)) CStep=3,sendData(1,2);
             if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,3);
             if((x<6750.0)&&(CStep==4)) CStep=5,sendData(1,5);
-            if((x<6300.0)&&(CStep==5)) CStep=7,sendData(1,4);
+//            if((x<6300.0)&&(CStep==5)) CStep=6,sendData(1,4);
+            if((x<6300.0)&&(CStep==5)) CStep=6,sendData(1,4);
 
 #else
             //Red
-            if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.0,step=1;
+            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=4;
+            if((step==3)&&(x<10.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*/
+                targ_velocity=-speed;
+                step=6;
+                sendData(1,4);
+            }
+            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((x>3344.0)&&(CStep==1)) CStep=2,sendData(1,1);
             if((x>6234.0)&&(CStep==2)) CStep=3,sendData(1,3);