Diff: main_ps3.cpp

Revision:

57:3fbd487e055e

Parent:

56:ac01d6b46291

Child:

58:6b73e683fa70

Child:

59:9d66edf3e734

--- a/main_ps3.cpp	Tue Sep 29 03:46:22 2015 +0000
+++ b/main_ps3.cpp	Tue Sep 29 08:23:03 2015 +0000
@@ -4,9 +4,18 @@
 
 //速度コントローラと向きコントローラはそのまま
 
+/***コース選択***/
 #define BLUE
 //#define RED
 
+/***コントローラ選択***/
+//#define IM920
+#define PS3
+
+#if defined(IM920) && defined(PS3)
+#error Caution, You should define either IM920 or PS3
+#endif
+
 #if defined(BLUE) && defined(RED)
 #error Caution, You should define either BLUE or RED
 #endif
@@ -18,7 +27,11 @@
 
 int main()
 {
+#ifdef IM920
     initializeIM920();
+#else
+    initializeSBDBT();
+#endif
     initializeMotors();
     initializeControllers();
     initializeSwing();
@@ -39,7 +52,11 @@
 //    double time=0.0;
     while(1) {
         if(autoflag){
+#ifdef IM920
             autoIM920(); /*IM920 button*/
+#else
+            autoPS3();   /*PS3 button*/
+#endif
 #ifdef BLUE
             //Blue
             if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.0,step=1;
@@ -69,8 +86,13 @@
             move_following();
         }
         else if(!autoflag) {
+#ifdef IM920
             manualMoveIM920(); /*analogStick*/
             manualIM920();     /*IM920 button*/
+#else
+            manualMovePS3();   /*analogStick*/
+            manualPS3();       /*PS3 button*/
+#endif
             //Swing
             swingFollowing();
             /*if(square){
@@ -81,13 +103,17 @@
             time+=0.01;*/
         }
         /***update state***/ 
+#ifdef IM920
         readIM920();
+#endif
         mesure_state();
         mesureSwing();
         wait(RATE);
 
 //        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);
+//        pc.printf("%f %f\r\n",cont,swingRadVelocity);
+        double pt=(2 * PI) / swingRadVelocity;
+        pc.printf("%f\r\n",pt);
     }
 }
\ No newline at end of file