Diff: main_ps3.cpp

Revision:

34:aa2a5c888a27

Parent:

33:a4323c20494b

Child:

35:7b6786193aa2

--- a/main_ps3.cpp	Thu Nov 12 07:23:30 2015 +0000
+++ b/main_ps3.cpp	Fri Nov 13 08:51:09 2015 +0000
@@ -73,27 +73,27 @@
             }
             
             //Cylinder
-            if((x>3250.0)&&(CStep==0)) { 
+            if((x>3200.0)&&(CStep==0)) {
                 if(!skip) sendData(1,1);
                 CStep=1; 
             }
-            if((x>6200.0)&&(CStep==1)) {
+            if((x>6150.0)&&(CStep==1)) {
                 if(!skip) sendData(1,2);
                 CStep=2;
             }
-            if((x>7800.0)&&(CStep==2)) {
+            if((x>7750.0)&&(CStep==2)) {
                 if(!skip) sendData(1,3);
                 CStep=3;
             }
-            if((x<7000.0)&&(CStep==3)) {
+            if((x<7050.0)&&(CStep==3)) {
                 if(!skip) sendData(1,4); //over
                 CStep=4;
             }
-            if((x<6920.0)&&(CStep==4)) {
+            if((x<6950.0)&&(CStep==4)) {
                 if(!skip) sendData(1,5); //middle
                 CStep=5;
             }
-            if((x<6900.0)&&(CStep==5)) {
+            if((x<6950.0)&&(CStep==5)) {
                 if(!skip) sendData(1,6); //front
                 CStep=6;
             }
@@ -149,18 +149,77 @@
                 CStep=114;
             }
 #else
+            /********************************Own & Middle Mode*********************************/
+            if((step==0)&&((10000.0>x)&&(x>1000.0))) {
+                targ_sita=0.03;
+//                targ_sita=0.0;
+                step=1;
+            }
+            if((step==1)&&(x>9000.0)) {
+                targ_velocity=0.0;
+                velocity_controller.setBias(0.0);
+                velocity_controller.reset();
+//                dpcount=speed;
+                step=2;
+            }
+            if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
+                step=3;
+                spcount=0.0;
+                flagf=0;
+//                targ_sita=0.0;
+                targ_sita=-0.03;
+            }
+            if((step==3)&&(x<1700.0)) {
+                targ_sita=-PI/4;
+                step=4;
+            }
+            if((step==4)&&(x<800.0)) {
+                dpcount=speed;
+                step=114;
+            }
+            
+            //Cylinder
+            if((x>3250.0)&&(CStep==0)) { 
+                if(!skip) sendData(1,1);
+                CStep=1; 
+            }
+            if((x>6200.0)&&(CStep==1)) {
+                if(!skip) sendData(1,3);
+                CStep=2;
+            }
+            if((x>7800.0)&&(CStep==2)) {
+                if(!skip) sendData(1,2);
+                CStep=3;
+            }
+            if((x<7000.0)&&(CStep==3)) {
+                if(!skip) sendData(1,6); //over
+                CStep=4;
+            }
+            if((x<6920.0)&&(CStep==4)) {
+                if(!skip) sendData(1,5); //middle
+                CStep=5;
+            }
+            if((x<6900.0)&&(CStep==5)) {
+                if(!skip) sendData(1,4); //front
+                CStep=6;
+            }
+            if((x<3000.0)&&(CStep==6)){
+                sendData(7,0);
+                CStep=114;
+            }
 #endif
         }
 //        else if(!modeflag) {
         else if(!autoflag){
             manualIM920();     /*IM920 button*/
+#ifdef BLUE
             /********************************Swing Mode*********************************/
             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;
+                targ_sita=PI/4.0;
                 mstep=114;
             }
             
@@ -194,9 +253,11 @@
                 sendData(1,4);
                 mCStep=114;
             }*/
+#else
+#endif
         }
-        pc.printf("b:%d\r\n",swingRadVelocity);
+//        pc.printf("b:%d\r\n",swingRadVelocity);
 //        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);
+        pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
     }
 }
\ No newline at end of file