Diff: main_ps3.cpp

Revision:

30:cd344beb415d

Parent:

29:460e49e37048

Child:

31:897fbd97d264

--- a/main_ps3.cpp	Tue Nov 10 07:46:00 2015 +0000
+++ b/main_ps3.cpp	Tue Nov 10 09:42:33 2015 +0000
@@ -42,16 +42,14 @@
         if(autoflag){
             autoIM920(); /*IM920 button*/
 #ifdef BLUE
-            /********************************Nomal Mode*********************************/
+            /********************************Own & Middle Mode*********************************/
             if((step==0)&&((10000.0>x)&&(x>800.0))) {
                 targ_sita=-0.06;
 //                targ_sita=0.0;
                 step=1;
             }
-            if((step==1)&&(x>10000.0)) {
+            if((step==1)&&(x>9000.0)) {
                 targ_velocity=0.0;
-                /*direction_controller.setBias(0.0);
-                direction_controller.reset();*/
                 velocity_controller.setBias(0.0);
                 velocity_controller.reset();
 //                dpcount=speed;
@@ -68,41 +66,99 @@
                 targ_sita=PI/4;
                 step=4;
             }
-            if((step==4)&&(x<600.0)) {
+            if((step==4)&&(x<500.0)) {
                 dpcount=speed;
                 step=114;
             }
             
-            /***Cylinder***/
-            if((x>3100.0)&&(CStep==1)) { 
+            //Cylinder
+            if((x>3100.0)&&(CStep==0)) { 
                 if(!skip) sendData(1,1);
-                CStep=2; 
+                CStep=1; 
             }
-            if((x>6000.0)&&(CStep==2)) {
+            if((x>6000.0)&&(CStep==1)) {
+                if(!skip) sendData(1,2);
+                CStep=2;
+            }
+            if((x>7700.0)&&(CStep==2)) {
                 if(!skip) sendData(1,3);
                 CStep=3;
             }
-            if((x>7700.0)&&(CStep==3)) {
-                if(!skip) sendData(1,2);
+            if((x<7300.0)&&(CStep==3)) {
+                if(!skip) sendData(1,5); //middle
                 CStep=4;
             }
-            if((x>10000.0)&&(CStep==4)) {
+            if((x<6650.0)&&(CStep==4)) {
+                if(!skip) sendData(1,4); //over
                 CStep=5;
             }
-            if((x<9300.0)&&(CStep==5)) {
-                if(!skip) sendData(1,5);
+            if((x<6200.0)&&(CStep==5)) {
+                if(!skip) sendData(1,6); //front
                 CStep=6;
             }
-            if((x<7450.0)&&(CStep==6)) {
-                if(!skip) sendData(1,6);
-                CStep=7;
-            }
-            if((x<5350.0)&&(CStep==7)) {
-                if(!skip) sendData(1,4);
-                CStep=8;
+            if((x<3000.0)&&(CStep==6)){
+                sendData(7,0);
+                CStep=114;
             }
             
-            if((x<1000.0)&&(CStep==8)){
+            /********************************Own & Opponent Mode*********************************/
+            if((step==10)&&((10000.0>x)&&(x>800.0))) {
+                targ_sita=-0.06;
+//                targ_sita=0.0;
+                step=11;
+            }
+            if((step==11)&&(x>10000.0)) {
+                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))){
+                step=13;
+                spcount=0.0;
+                flagf=0;
+//                targ_sita=0.0;
+                targ_sita=-0.06;
+            }
+            if((step==13)&&(x<1400.0)) {
+                targ_sita=PI/4;
+                step=14;
+            }
+            if((step==14)&&(x<600.0)) {
+                dpcount=speed;
+                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);
+                CStep=12;
+            }
+            if((x>7700.0)&&(CStep==12)) {
+                if(!skip) sendData(1,3);
+                CStep=13;
+            }
+            if((x>10000.0)&&(CStep==13)) {
+                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)){
                 sendData(7,0);
                 CStep=114;
             }
@@ -110,16 +166,15 @@
 #endif
         }
         else if(!autoflag) {
-            flaga=0;
             manualIM920();     /*IM920 button*/
-//            mesureSwing();
-//            swingFollowing();
-//            wait(RATE);
-//            fprintf(fp_r, "%f\r\n",swingRadVelocity);
-//            if(b==11) fclose(fp_r);
+            /********************************Swing Mode*********************************/
+            if((mstep==0)&&((10000.0>x)&&(x>1500.0))) {
+                dpcount=speed;
+                mstep=114;
+            }
+            
             pc.printf("Swing:%f\r\n",swingRadVelocity);
         }
-        /***update state***/ 
 //        pc.printf("Swing:%f\r\n",SwingSens.getPulses());
 //        pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
     }