Diff: main_ps3.cpp

Revision:

108:7eb434cfcbd7

Parent:

107:579bb1ab67d9

Child:

109:e009296125db

--- a/main_ps3.cpp	Mon Oct 19 08:29:58 2015 +0000
+++ b/main_ps3.cpp	Mon Oct 19 11:55:00 2015 +0000
@@ -13,6 +13,7 @@
 //モード切替時に時間を待つ
 //赤チーム側を青チーム側のプログラムに合わせて実装
 //横幅調整済み
+//相手妨害モードいらない new!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 
 /***コース選択***/
 //#define BLUE
@@ -113,7 +114,7 @@
                 if(!skip) sendData(1,1);
                 CStep=2; 
             }
-            if((x>5890.0+deff)&&(CStep==2)) {
+            if((x>5900.0+deff)&&(CStep==2)) {
                 if(!skip) sendData(1,3);
                 CStep=3;
             }
@@ -121,7 +122,7 @@
                 if(!skip) sendData(1,2);
                 CStep=4;
             }
-            if((x<6730.0+deff)&&(CStep==4)) {
+            if((x<6650.0+deff)&&(CStep==4)) {
                 if(!skip) sendData(1,5);
                 CStep=5;
             }
@@ -194,13 +195,14 @@
                 step=114;
             }
             
-//            if((x>1350.0)&&(CStep==15)){
-            if((x>1200.0+deff)&&(CStep==15)){
+//            if((x>1200.0+deff)&&(CStep==15)){
+            if((x>1000.0+deff)&&(CStep==15)){
                 sendData(1,5);
                 CStep=16;
             }
-            if((x>2550.0+deff)&&(CStep==16)){
-//            if((x>2670.0)&&(CStep==16)){
+//            if((x>2655.0+deff)&&(CStep==16)){
+            if((x>2455.0+deff)&&(CStep==16)){
+//            if((x>2630.0)&&(CStep==16)){
                 sendData(1,4);
                 CStep=17;
             }
@@ -214,7 +216,7 @@
                 targ_sita=0.025;
                 step=51;
             }
-            if((step==51)&&(x>1000.0+deff)) {
+            if((step==51)&&(x>1500.0+deff)) {
                 targ_velocity=0.0;
                 velocity_controller.reset();
                 direction_controller.reset();
@@ -226,6 +228,7 @@
             if((step==52)&&((sita<PI/2.0+0.5)&&(sita>PI/2.0-0.5))){
                 step=53;
                 autoflag=0;
+                flaga=0;
             }
             /////////////////////////////////////////////////////////////////////////
 #else       
@@ -359,7 +362,26 @@
                 sendData(7,0);
                 CStep=114;
             }
-            
+            /////////////////////////////////////////////////////////////////////////
+            /*****************************Disturb Mode****************************/
+            if((step==50)&&((8650.0>x)&&(x>800.0))) {
+                targ_sita=-0.025;
+                step=51;
+            }
+            if((step==51)&&(x>1000.0+deff)) {
+                targ_velocity=0.0;
+                Move_l(0.0);
+                Move_r(0.0);
+                velocity_controller.reset();
+                direction_controller.reset();
+                targ_sita=-PI/2.0;
+                step=52;
+            }
+            if((step==52)&&((sita<PI/2.0+0.5)&&(sita>PI/2.0-0.5))){
+                step=53;
+                autoflag=0;
+                flaga=0;
+            }
             /////////////////////////////////////////////////////////////////////////
 #endif
             mesure_state();   /*位置測定*/
@@ -391,7 +413,7 @@
 #endif
         mesureSwing();
         wait(RATE);
-        pc.printf("%f\r\n",(float)((((2.0 * PI) / swingRadVelocity) / 4.0)));
+//        pc.printf("%f\r\n",(float)((((2.0 * PI) / swingRadVelocity) / 4.0)));
 //        pc.printf("sita:%f, x:%f, y:%f ,x1:%f, x2:%f ,velocity:%f\r\n",sita,x,y,x1,x2,velocity);
 //        pc.printf("A2 = %d, X = %d, Y = %d, B = %d, dead = %d\r\n", a2, X, Y, b, deadflag);
 //        pc.printf("%f %f %f\r\n",cont,swingRadVelocity, (double)SwingSens.getPulses());