Diff: User.cpp

Revision:

16:03dd8c0a8d1d

Parent:

15:c5258a4dee75

--- a/User.cpp	Sat Apr 29 11:48:36 2017 +0000
+++ b/User.cpp	Thu May 18 15:29:54 2017 +0000
@@ -7,20 +7,38 @@
 #include "mbed.h"
 
 int RSX,RSY,LSX,LSY,BSU,BSL;
+int servo2value;
+
 //これより下に関数外に書く要素を記入する
-PwmOut led1(LED1);
-PwmOut led2(LED2);
-DigitalOut led3(LED3);
-DigitalOut led4(LED4);
+PwmOut servo1(p21);
+PwmOut servo2(p22);
+PwmOut left1(p23);
+PwmOut left2(p24);
+PwmOut right1(p25);
+PwmOut right2(p26);
 
-void UserLoopSetting(){
+void UserLoopSetting()
+{
+    servo2value = 950;
+    
     //一度だけ行いたい初期設定をここに書く
-    led1.period_ms(20);
+    
+    servo1.period_ms(20);
+    servo2.period_ms(20);
+
+    servo1.pulsewidth_us(950);
+    servo2.pulsewidth_us(servo2value);
+
+    left1 = 0;
+    left2 = 0;
+    right1 = 0;
+    right2 = 0;
 }
 
-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 +47,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;
@@ -39,25 +57,53 @@
         //ボタンの処理
         ButtonState =  ((ps3report*)(data + 1))->ButtonState;
     }
-    //ここより下にプログラムを書く
-    
-    
-    //データ取得例
+
+    //○が押されたとき
     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の状態
+        servo1.pulsewidth_us(750);
+    } else if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {
+        servo1.pulsewidth_us(1200);
+    } else if((ButtonState >> BUTTONCROSS)&1 == 1) {
+        servo1.pulsewidth_us(1800);
+    }
+
+    if((ButtonState >> BUTTONL1)&1 == 1) {
+        ++servo2value;
+        servo2.pulsewidth_us(servo2value);
+    } else if((ButtonState >> BUTTONR1)&1 == 1) {
+        --servo2value;
+        servo2.pulsewidth_us(servo2value);
+    }
+
+    if(LSY > 130) {
+        left2 = ((LSY - 128) / 128.0f * 0.8f);
+        left1 = 0;
+    } else if(LSY < 120) {
+        left1= (LSY / 128.0f * 0.8f);
+        left2 = 0;
+    } else {
+        left1 = 0;
+        left2 = 0;
+    }
+
+    if(RSY>130) {
+        right2 = ((RSY - 128)/ 128.0f * 0.8f);
+        right1 = 0;
+    } else if(LSY < 120) {
+        right1 = (RSY / 128.0f * 0.8f);
+        right2 = 0;
+    } else {
+        right1 = 0;
+        right2 = 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