Diff: User.cpp

Revision:

18:299810a6dcb4

Parent:

17:ae30e74e9eba

--- a/User.cpp	Thu Mar 10 03:23:21 2016 +0000
+++ b/User.cpp	Thu Mar 10 04:51:16 2016 +0000
@@ -18,21 +18,22 @@
 int RSX,RSY,LSX,LSY,BSU,BSL;
 //これより下に関数外に書く要素を記入する
 DigitalOut LA(p18);
-DigitalOut RA(p19);
-DigitalOut LH(p20);
-DigitalOut RH(p21);
-DigitalOut CH(p22);
+DigitalOut RA(p21);
+DigitalOut LH(p19);
+DigitalOut RH(p20);
+DigitalOut CH(p23);
 PwmOut motor[2] = {p25,p26};
 SPI m(p5, p6, p7);
 DigitalOut cs(p8);
 //Serial pc(USBTX,USBRX);
 
 DigitalOut leds[4] = {LED1,LED2,LED3,LED4};
-
+int countA=0,countB=0,countC=0,countD=0;
 
 
 //
-void UserLoopSetting(){
+void UserLoopSetting()
+{
     //一度だけ行いたい初期設定をここに書く
     LA=0;
     RA=0;
@@ -42,9 +43,10 @@
     initSerialServo();
 }
 
-void UserLoop(char n,const u8* data){
+void UserLoop(char n,const u8* data)
+{
     u16 ButtonState;
-    if(n==0){//有線Ps3USB.cpp
+    if(n==0) { //有線Ps3USB.cpp
         RSX = ((ps3report*)data)->RightStickX;
         RSY = ((ps3report*)data)->RightStickY;
         LSX = ((ps3report*)data)->LeftStickX;
@@ -53,7 +55,7 @@
         BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
         //ボタンの処理
         ButtonState =  ((ps3report*)data)->ButtonState;
-    }else {//無線TestShell.cpp
+    } else {//無線TestShell.cpp
         RSX = ((ps3report*)(data + 1))->RightStickX;
         RSY = ((ps3report*)(data + 1))->RightStickY;
         LSX = ((ps3report*)(data + 1))->LeftStickX;
@@ -65,110 +67,127 @@
     }
     //ここより下にプログラムを書く
     //データ取得例
-    
+
     int M=0;
     m.format(16,3);
     m.frequency(1000000);
     cs=0;
-/*    if((ButtonState >> BUTTONCIRCLE)&1) {
-        //○が押されたとき
-    }
-    */
-    if((ButtonState >> BUTTONUP)&1){
+    /*    if((ButtonState >> BUTTONCIRCLE)&1) {
+            //○が押されたとき
+        }
+        */
+    if((ButtonState >> BUTTONUP)&1) {
         motor[0]=0;
         motor[1]=0.1;
-        }
-    else if((ButtonState >> BUTTONDOWN)&1){
+    } else if((ButtonState >> BUTTONDOWN)&1) {
         motor[1]=0;
         motor[0]=0.1;
-        }
-    else{
+    } else {
         motor[0]=0;
         motor[1]=0;
-        }
-/*    if((ButtonState >> BUTTONRIGHT)&1){
-        set_degree(ID,50);
-    }else if((ButtonState >> BUTTONLEFT)&1){
-        set_degree(ID,200);
-    }else{*/
-    if(servoflag == 0){
+    }
+    /*    if((ButtonState >> BUTTONRIGHT)&1){
+            set_degree(ID,50);
+        }else if((ButtonState >> BUTTONLEFT)&1){
+            set_degree(ID,200);
+        }else{*/
+    if(servoflag == 0) {
         set_degree(ID,135);
         servoflag= 1;
-        }
+    }
 //    }
-    if((ButtonState >> BUTTONL2)&1){
-        LA=!LA;
+    if((ButtonState >> BUTTONL2)&1) {
+        countA++;
+        if(countA>50) {
+            LA=!LA;
+            RA=!RA;
+            countA=0;
         }
-    else if((ButtonState >> BUTTONR2)&1){
-        RA=!RA;
+    } else if((ButtonState >> BUTTONR2)&1) {
+        countA++;
+        if(countA>50) {
+            RA=!RA;
+            LA=!LA;
+            countA=0;
         }
-    else if((ButtonState >> BUTTONCROSS)&1){
-        RA=!RA;
-        LA=!LA;
-        }
-    else if((ButtonState >> BUTTONCIRCLE)&1){
-        RH=!RH;
+    } else if((ButtonState >> BUTTONCROSS)&1) {
+        countA++;
+        if(countA>50) {
+            RA=!RA;
+            LA=!LA;
+            countA=0;
         }
-    else if((ButtonState >> BUTTONTRIANGEL)&1){
-        CH=!CH;
+    } else if((ButtonState >> BUTTONCIRCLE)&1) {
+        countB++;
+        if(countB>50) {
+            RH=!RH;
+            countB=0;
         }
-    else if((ButtonState >> BUTTONSQUARE)&1){
-        LH=!LH;
-        }
-    if((ButtonState >> BUTTONR1)&1){
-        M+=1 << 15;
+    } else if((ButtonState >> BUTTONTRIANGEL)&1) {
+        countC++;
+        if(countC>50) {
+            CH=!CH;
+            LH=!LH;
+            countC=0;
         }
-    else if((ButtonState >> BUTTONL1)&1){
-        M+=1 << 14;
+    } else if((ButtonState >> BUTTONSQUARE)&1) {
+        countD++;
+        if(countD>50) {
+            LH=!LH;
+            CH=!CH;
+            countD=0;
         }
-    if(LSX-128>=0){
-        M+=1 << 13;
+    }
+    if((ButtonState >> BUTTONR1)&1) {
+        M+=1 << 15;
+    } else if((ButtonState >> BUTTONL1)&1) {
+        M+=1 << 14;
+    }
+    if(LSX-128>=0) {
         M+=((LSX-128)/2) << 7;
-        }
-    else if(LSX-128<0){
+    } else if(LSX-128<0) {
+        M+=1 << 13;
         if(128-LSX==128)M+=63 << 7;
         else M+=((128-LSX)/2) << 7;
-        }
-    if(LSY-127<=0){
+    }
+    if(LSY-127<=0) {
+        M+=((127-LSY)/2);
+    } else if(LSY-127>0) {
         M+=1 << 6;
-        M+=((127-LSY)/2);
-        }
-    else if(LSY-127>0){
         if(LSY-127==128)M+=63;
         else M+=(LSY-127)/2;
-        }
-    #ifdef DEBUG_MODE
+    }
+#ifdef DEBUG_MODE
     int MM,i,MA[16];
     MM=M;
-    for(i=0;MM>0;i++){
+    for(i=0; MM>0; i++) {
         MA[i]=MM % 2;
         MM=MM/2;
     }
-    for(i=i+1;i<16;i++){
+    for(i=i+1; i<16; i++) {
         MA[i]=0;
     }
-    for(i=15;i>=0;i--){printf("%d",MA[i]);}
+    for(i=15; i>=0; i--) {
+        printf("%d",MA[i]);
+    }
     printf("\r\n");
-    #endif    
+#endif
     //printf("LSX=%d,LSY=%d\r\n",LSX,LSY);
     printf("M=%d \r\n",M);
-    
-    m.write(M);    
-    
-/*
-    led1=LSX/256.0f;
-    led2=LSY/256.0f;
-    led3=ButtonState & 0x0400;  //L1の状態
-    led4=ButtonState & 0x0800;  //R1の状態
-    */
-    for(int i = 0;i<4;i++){
-        leds[i] = !leds[i];
-        wait_ms(50);
-        }
+
+    m.write(M);
+
+    /*
+        led1=LSX/256.0f;
+        led2=LSY/256.0f;
+        led3=ButtonState & 0x0400;  //L1の状態
+        led4=ButtonState & 0x0800;  //R1の状態
+        */
+
     //値の取得はps3.hを参照
     cs=1;
 //    setID(ID);
 //    readID();
 //    readID();
-    
+
 }
\ No newline at end of file