Diff: User.cpp

Revision:

16:b37dc2bb1352

Parent:

15:c5258a4dee75

Child:

17:41ea4c1bc6af

diff -r c5258a4dee75 -r b37dc2bb1352 User.cpp
--- a/User.cpp	Sat Apr 29 11:48:36 2017 +0000
+++ b/User.cpp	Wed May 24 10:45:33 2017 +0000
@@ -8,19 +8,41 @@
 
 int RSX,RSY,LSX,LSY,BSU,BSL;
 //これより下に関数外に書く要素を記入する
-PwmOut led1(LED1);
-PwmOut led2(LED2);
-DigitalOut led3(LED3);
-DigitalOut led4(LED4);
+
+
+//moter Pwm*2
+//右足
+PwmOut pin_ueasi_1(p25);
+PwmOut pin_ueasi_2(p26);
+//左足
+PwmOut pin_sitaasi_1(p23);
+PwmOut pin_sitaasi_2(p24);
 
-void UserLoopSetting(){
+//moter Pwm*1 DigitalOut*2//
+//採取腕//
+PwmOut pin_pick_1(p22);
+DigitalOut pin_pick_2(p20);
+DigitalOut pin_pick_3(p19);
+//振幅腕//
+PwmOut pin_lift_1(p21);
+DigitalOut pin_lift_2(p18);
+DigitalOut pin_lift_3(p17);
+
+double ue_speed=0.9;
+double sita_speed=1;
+double pick_speed=1;
+double lift_speed=1;
+
+
+void UserLoopSetting()
+{
     //一度だけ行いたい初期設定をここに書く
-    led1.period_ms(20);
 }
 
-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;
@@ -29,7 +51,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;
@@ -40,24 +62,133 @@
         ButtonState =  ((ps3report*)(data + 1))->ButtonState;
     }
     //ここより下にプログラムを書く
+
+    //ーー足回りーー
+    //右足
+    if((ButtonState >> BUTTONRANALOG)&1 == 1)
+        ue_speed=0.9;
+    else
+        ue_speed=0.45;
+    if(RSY>=0 && RSY<=80) { //正回転
+        pin_ueasi_1=ue_speed;
+        pin_ueasi_2=0;
+
+    } else if(RSY>=150&&RSY<=255) { //負回転
+        pin_ueasi_1=0;
+        pin_ueasi_2=ue_speed;
+    } else { //停止
+        pin_ueasi_1=0;
+        pin_ueasi_2=0;
+    }
+
+
+
+    //左足
+    if((ButtonState >> BUTTONLANALOG)&1 == 1)
+        sita_speed=1;
+    else
+        sita_speed=0.5;
+    if(LSY>=0 && LSY<=80) {//正回転
+        pin_sitaasi_1=0;
+        pin_sitaasi_2=sita_speed;
+    } else if(LSY>=150 && LSY<=255) { //負回転
+        pin_sitaasi_1=sita_speed;
+        pin_sitaasi_2=0;
+    } else { //停止
+        pin_sitaasi_1=0;
+        pin_sitaasi_2=0;
+    }
+
+
+
+
+    //採取腕
+    if((ButtonState >> BUTTONR1)&1 == 1) {//掴み
+        pin_pick_1=pick_speed;
+        pin_pick_2=0;
+        pin_pick_3=1;
+    } else if((ButtonState >> BUTTONR2)&1 == 1) { //離し
+        pin_pick_1=pick_speed;
+        pin_pick_2=1;
+        pin_pick_3=0;
+    } else { //停止
+        pin_pick_1=0;
+        pin_pick_2=0;
+        pin_pick_3=1;
+    }
+
+    //振幅腕
+    if((ButtonState >> BUTTONL1)&1 == 1) {//上昇
+        pin_lift_1=lift_speed;
+        pin_lift_2=1;
+        pin_lift_3=0;
+
+
+
+    } else if((ButtonState >> BUTTONL2)&1 == 1) { //下降
+        pin_lift_1=lift_speed;
+        pin_lift_2=0;
+        pin_lift_3=1;
+        pin_pick_1=pick_speed;
+        pin_pick_2=0;
+        pin_pick_3=1;
+
+    } else { //停止
+        pin_lift_1=0;
+        pin_lift_2=1;
+        pin_lift_3=0;
+    }
+
+
+//〇ボタンで一個掃き出し
+    if((ButtonState >> BUTTONCIRCLE)&1 == 1) {
+         //離し
+        pin_pick_1=pick_speed;
+        pin_pick_2=1;
+        pin_pick_3=0;
+        wait(0.5);
+         //誤差修正
+        pin_pick_1=pick_speed;
+        pin_pick_2=0;
+        pin_pick_3=1;
+        wait(0.4);
+
+
+    }
+   
+
+    //十字キー
+    if((ButtonState >> BUTTONUP)&1 == 1) {//上ボタン 前進
+        pin_ueasi_1=ue_speed;
+        pin_ueasi_2=0;
+        pin_sitaasi_1=0;
+        pin_sitaasi_2=sita_speed;
+    } if((ButtonState >> BUTTONDOWN)&1 == 1) {//下ボタン　後進
+        pin_ueasi_1=0;
+        pin_ueasi_2=ue_speed;
+        pin_sitaasi_1=sita_speed;
+        pin_sitaasi_2=0;
+    } if((ButtonState >> BUTTONRIGHT)&1 == 1) {//右ボタン　右旋回
+        pin_ueasi_1=0;
+        pin_ueasi_2=ue_speed;
+        pin_sitaasi_1=0;
+        pin_sitaasi_2=sita_speed;
+    } if((ButtonState >> BUTTONLEFT)&1 == 1) {//左ボタン　左旋回
+        pin_ueasi_1=ue_speed;
+        pin_ueasi_2=0;
+        pin_sitaasi_1=sita_speed;
+        pin_sitaasi_2=0;
+    }
     
     
+    
+
+
+
     //データ取得例
-    if((ButtonState >> BUTTONCIRCLE)&1 == 1) {
-        led1 = 1;
-        //○が押されたとき
-    }else{
-        led1 = 0;
-        //○を押してないとき
-        }
-        
-    led1=LSX/256.0f;
-    led2=LSY/256.0f;
-    led3=ButtonState & 0x0400;  //L1の状態
-    led4=ButtonState & 0x0800;  //R1の状態
+
     //値の取得はps3.hを参照
     //ここまでプログラム例    実機に乗せるときは消して大丈夫です
-    
-    
-        
-}
\ No newline at end of file
+
+
+}