Diff: main_ps3.cpp

Revision:

33:a4323c20494b

Parent:

32:b8c8ad2eeca7

Child:

34:aa2a5c888a27

--- a/main_ps3.cpp	Wed Nov 11 06:40:39 2015 +0000
+++ b/main_ps3.cpp	Thu Nov 12 07:23:30 2015 +0000
@@ -39,12 +39,13 @@
     wait(0.3);
     sendData(7,0);
     while(1) {
+//        if(modeflag){
         if(autoflag){
             autoIM920(); /*IM920 button*/
 #ifdef BLUE
             /********************************Own & Middle Mode*********************************/
-            if((step==0)&&((10000.0>x)&&(x>900.0))) {
-                targ_sita=-0.06;
+            if((step==0)&&((10000.0>x)&&(x>1000.0))) {
+                targ_sita=-0.03;
 //                targ_sita=0.0;
                 step=1;
             }
@@ -60,27 +61,27 @@
                 spcount=0.0;
                 flagf=0;
 //                targ_sita=0.0;
-                targ_sita=-0.06;
+                targ_sita=-0.03;
             }
-            if((step==3)&&(x<1400.0)) {
+            if((step==3)&&(x<1700.0)) {
                 targ_sita=PI/4;
                 step=4;
             }
-            if((step==4)&&(x<500.0)) {
+            if((step==4)&&(x<800.0)) {
                 dpcount=speed;
                 step=114;
             }
             
             //Cylinder
-            if((x>3100.0)&&(CStep==0)) { 
+            if((x>3250.0)&&(CStep==0)) { 
                 if(!skip) sendData(1,1);
                 CStep=1; 
             }
-            if((x>6000.0)&&(CStep==1)) {
+            if((x>6200.0)&&(CStep==1)) {
                 if(!skip) sendData(1,2);
                 CStep=2;
             }
-            if((x>7700.0)&&(CStep==2)) {
+            if((x>7800.0)&&(CStep==2)) {
                 if(!skip) sendData(1,3);
                 CStep=3;
             }
@@ -90,12 +91,10 @@
             }
             if((x<6920.0)&&(CStep==4)) {
                 if(!skip) sendData(1,5); //middle
-                wait(0.001);
-                if(!skip) sendData(1,6); //front
                 CStep=5;
             }
-            if((x<6850.0)&&(CStep==5)) {
-//                if(!skip) sendData(1,6); //front
+            if((x<6900.0)&&(CStep==5)) {
+                if(!skip) sendData(1,6); //front
                 CStep=6;
             }
             if((x<3000.0)&&(CStep==6)){
@@ -104,8 +103,8 @@
             }
             
             /********************************Own & Opponent Mode*********************************/
-            if((step==10)&&((10000.0>x)&&(x>800.0))) {
-                targ_sita=-0.06;
+            if((step==10)&&((10000.0>x)&&(x>700.0))) {
+                targ_sita=-0.03;
 //                targ_sita=0.0;
                 step=11;
             }
@@ -113,7 +112,6 @@
                 targ_velocity=0.0;
                 velocity_controller.setBias(0.0);
                 velocity_controller.reset();
-//                dpcount=speed;
                 step=12;
             }
             if((step==12)&&((velocity<500.0)&&(velocity>-500.0))){
@@ -121,7 +119,7 @@
                 spcount=0.0;
                 flagf=0;
 //                targ_sita=0.0;
-                targ_sita=-0.06;
+                targ_sita=-0.03;
             }
             if((step==13)&&(x<1400.0)) {
                 targ_sita=PI/4;
@@ -132,50 +130,70 @@
                 step=114;
             }
             
-            //Cylinder
-            if((x>3100.0)&&(CStep==10)) { 
-                if(!skip) sendData(1,1);
-                CStep=11; 
-            }
-            if((x>6000.0)&&(CStep==11)) {
-                if(!skip) sendData(1,2);
+            //Cylinderd
+            if((x>9500.0)&&(CStep==10)) CStep=11;
+            if((x<9400.0)&&(CStep==11)) {
+                if(!skip) sendData(1,4);
                 CStep=12;
             }
-            if((x>7700.0)&&(CStep==12)) {
-                if(!skip) sendData(1,3);
+            if((x<6900.0)&&(CStep==12)) {
+                if(!skip) sendData(1,5);
                 CStep=13;
             }
-            if((x>10000.0)&&(CStep==13)) {
+            if((x<5350.0)&&(CStep==13)) {
+                if(!skip) sendData(1,6);
                 CStep=14;
             }
-            if((x<9300.0)&&(CStep==14)) {
-                if(!skip) sendData(1,4);
-                CStep=15;
-            }
-            if((x<7450.0)&&(CStep==15)) {
-                if(!skip) sendData(1,5);
-                CStep=16;
-            }
-            if((x<5350.0)&&(CStep==16)) {
-                if(!skip) sendData(1,6);
-                CStep=17;
-            }
-            if((x<1000.0)&&(CStep==17)){
+            if((x<1000.0)&&(CStep==14)){
                 sendData(7,0);
                 CStep=114;
             }
 #else
 #endif
         }
-        else if(!autoflag) {
+//        else if(!modeflag) {
+        else if(!autoflag){
             manualIM920();     /*IM920 button*/
             /********************************Swing Mode*********************************/
-            if((mstep==0)&&((10000.0>x)&&(x>1000.0))) {
+            if((mstep==0)&&((10000.0>x)&&(x>1700.0))) {
                 dpcount=speed;
+                mstep=1;
+            }
+            if((mstep==1)&&((velocity<5.0)&&(velocity>-5.0))){
+                targ_velocity=PI/4.0;
                 mstep=114;
             }
             
-//            pc.printf("Swing:%f\r\n",swingRadVelocity);
+            /********************************Opponent Struct Mode*********************************/
+            if((mstep==10)&&((10000.0>x)&&(x>1150.0))) {
+                targ_sita=-0.03;
+//                targ_sita=0.0;
+                mstep=11;
+            }
+            /*if((mstep==11)&&(x>8000.0)) {
+                targ_velocity=0.0;   
+                mstep=13;
+            }*/
+            if((mstep==11)&&(x>8000.0)) {
+                targ_sita=-PI/4.0;   
+                mstep=12;
+            }
+            if((mstep==12)&&(x>8700.0)) {
+                targ_velocity=0.0;   
+                mstep=13;
+            }
+            if((mstep==13)&&((velocity<5.0)&&(velocity>-5.0))){
+                targ_sita=0.0;
+                mstep=14;
+            }
+            /*if((mstep==14)&&((sita<0.01)&&(sita>-0.01))){
+                sendData(1,6);
+                mstep=114;
+            }*/
+            /*if((mCStep==0)&&(x>7100.0)){
+                sendData(1,4);
+                mCStep=114;
+            }*/
         }
         pc.printf("b:%d\r\n",swingRadVelocity);
 //        pc.printf("Swing:%f\r\n",SwingSens.getPulses());