Diff: main_ps3.cpp

Revision:

58:6b73e683fa70

Parent:

57:3fbd487e055e

--- a/main_ps3.cpp	Tue Sep 29 08:23:03 2015 +0000
+++ b/main_ps3.cpp	Wed Sep 30 12:31:48 2015 +0000
@@ -23,7 +23,7 @@
 #include "machine_ps3.h"
 
 Serial pc(USBTX, USBRX);
-//LocalFileSystem local("local");
+LocalFileSystem local("local");
 
 int main()
 {
@@ -44,7 +44,7 @@
 #endif
     Indicator4=1;
     Enable=1;
-//    FILE *fp_r = fopen("/local/velocity.dat", "w");
+    FILE *fp_r = fopen("/local/velocity.dat", "w");
     sendData(4,31);
     wait(0.1);
     sendData(5,31);
@@ -59,29 +59,43 @@
 #endif
 #ifdef BLUE
             //Blue
-            if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.0,step=1;
-            if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2;
+            if((step==0)&&((8650.0>x)&&(x>1380.0))) targ_sita=0.0,step=1;
+            if((step==1)&&(x>8550.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,autoflag=0;
             
             if((x>3344.0)&&(CStep==1)) CStep=2,sendData(1,1);
             if((x>6234.0)&&(CStep==2)) CStep=3,sendData(1,2);
             if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,3);
-//            if((x<6700.0)&&(CStep==4)) CStep=5,sendData(1,5);
             if((x<6750.0)&&(CStep==4)) CStep=5,sendData(1,5);
-            if((x<6300.0)&&(CStep==5)) CStep=6,sendData(1,4);
+            if((x<6300.0)&&(CStep==5)) CStep=0,sendData(1,4);
+            
+            //後でデバッグ
+            /*if((step==4)&&((6500.0>x)&&(x>1400.0))) targ_sita=0.0,step=5;
+            if((step==5)&&(x>6500.0)) targ_velocity=-speed,step=6;
+            if((step==6)&&(x<2000.0)) targ_sita=PI/4,step=7;
+            if((step==7)&&(x<10.0)) targ_velocity=0.0,autoflag=0;
+            
+            if((x>6000.0)&&(CStep==4)) CStep=0,sendData(1,5);*/
 #else
             //Red
-            if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.0,step=1;
+            if((step==0)&&((8650.0>x)&&(x>1380.0))) targ_sita=0.0,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=4,autoflag=0;
+
             if((x>3344.0)&&(CStep==1)) CStep=2,sendData(1,1);
             if((x>6234.0)&&(CStep==2)) CStep=3,sendData(1,3);
             if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,2);
-            if((x<6700.0)&&(CStep==4)) CStep=5,sendData(1,4);
-            if((x<6300.0)&&(CStep==5)) CStep=6,sendData(1,5);
+            if((x<6750.0)&&(CStep==4)) CStep=5,sendData(1,4);
+            if((x<6300.0)&&(CStep==5)) CStep=0,sendData(1,5);
+            
+            /*if((step==4)&&((6500.0>x)&&(x>1400.0))) targ_sita=0.0,step=5;
+            if((step==5)&&(x>6500.0)) targ_velocity=-speed,step=6;
+            if((step==6)&&(x<2000.0)) targ_sita=-PI/4,step=7;
+            if((step==7)&&(x<10.0)) targ_velocity=0.0,autoflag=0;
+            
+            if((x>6000.0)&&(CStep==4)) CStep=0,sendData(1,4);*/
 #endif            
             move_following();
         }
@@ -94,7 +108,7 @@
             manualPS3();       /*PS3 button*/
 #endif
             //Swing
-            swingFollowing();
+//            swingFollowing();
             /*if(square){
                 IndicatorAuto=0;
                 fclose(fp_r);
@@ -102,6 +116,8 @@
             fprintf(fp_r, "time:%1.3f, %f[rad/s], pwm:%f\r\n",time,swingRadVelocity,cont);
             time+=0.01;*/
         }
+        fprintf(fp_r, "%d, %f, %f\r\n",step,x,sita);
+        if(triangle) fclose(fp_r);
         /***update state***/ 
 #ifdef IM920
         readIM920();
@@ -112,8 +128,6 @@
 
 //        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\r\n",cont,swingRadVelocity);
-        double pt=(2 * PI) / swingRadVelocity;
-        pc.printf("%f\r\n",pt);
+        pc.printf("%f %f\r\n",cont,swingRadVelocity);
     }
 }
\ No newline at end of file