Diff: main_ps3.cpp

Revision:

52:d9e1629da852

Parent:

51:cb430192b28b

Child:

55:db1797ac6cb1

--- a/main_ps3.cpp	Sun Sep 27 05:02:15 2015 +0000
+++ b/main_ps3.cpp	Sun Sep 27 10:04:52 2015 +0000
@@ -4,8 +4,8 @@
 
 //速度コントローラと向きコントローラはそのまま
 
-//#define BLUE
-#define RED
+#define BLUE
+//#define RED
 
 #if defined(BLUE) && defined(RED)
 #error Caution, You should define either BLUE or RED
@@ -18,7 +18,6 @@
 
 int main()
 {
-//    initializeSBDBT();
     initializeIM920();
     initializeMotors();
     initializeControllers();
@@ -31,13 +30,14 @@
     Indicator4=1;
     Enable=1;
 //    FILE *fp_r = fopen("/local/velocity.dat", "w");
-    /*sendData(4,31);
+    sendData(4,31);
     wait(0.1);
-    sendData(5,31);*/
+    sendData(5,31);
+    wait(0.1);
 //    double time=0.0;
     while(1) {
         if(autoflag){
-            autoPs3(); /*ps3 button*/
+            autoIM920(); /*IM920 button*/
 #ifdef BLUE
             //Blue
             if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.0,step=1;
@@ -48,8 +48,9 @@
             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<6300.0)&&(CStep==4)) CStep=5,sendData(1,5);
-            if((x<5900.0)&&(CStep==5)) CStep=6,sendData(1,4);
+//            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);
 #else
             //Red
             if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.0,step=1;
@@ -60,17 +61,15 @@
             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<6074.0)&&(CStep==4)) CStep=5,sendData(1,4);
-            if((x<5574.0)&&(CStep==5)) CStep=6,sendData(1,5);
+            if((x<6700.0)&&(CStep==4)) CStep=5,sendData(1,4);
+            if((x<6300.0)&&(CStep==5)) CStep=6,sendData(1,5);
 //            pc.printf("sita:%f, x:%f, y:%f ,x1:%f, x2:%f ,velocity:%f\r\n",sita,x,y,x1,x2,velocity);
 #endif            
             move_following();
-//            pc.printf("sita:%f, x:%f, y:%f ,x1:%f, x2:%f ,velocity:%f\r\n",sita,x,y,x1,x2,velocity);
-//            pc.printf("r:%ld, l:%ld\r\n",Move_r_sense.getPulses(),Move_l_sense.getPulses());
         }
         else if(!autoflag) {
-            manualMovePs3(); /*analogStick*/
-            manualPs3();     /*ps3 button*/
+            manualMoveIM920(); /*analogStick*/
+            manualIM920();     /*IM920 button*/
             //Swing
             swingFollowing();
             /*if(square){
@@ -86,7 +85,7 @@
         mesureSwing();
         wait(RATE);
         
-        pc.printf("A2 = %d, X = %d, Y = %d, B = %d dead = %d\r\n", a2, X, Y, b, deadflag);
+//        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);
     }
 }
\ No newline at end of file