Diff: momoi_taka.cpp

Revision:

13:9e04b53e875a

Parent:

12:d811e29e469a

--- a/momoi_taka.cpp	Thu Jun 09 11:58:32 2016 +0000
+++ b/momoi_taka.cpp	Fri Jun 10 17:27:13 2016 +0000
@@ -3,54 +3,67 @@
 #include "hci.h"
 #include "ps3.h"
 #include "User.h"
-
 #include "mbed.h"
 
 int RSX,RSY,LSX,LSY,BSU,BSL;
 //これより下に関数外に書く要素を記入する
+double flag;
+//MD1
+PwmOut motorRightOne (p23);
+PwmOut motorRightTwo (p24);
+
+//MD2
+PwmOut motorLeftOne (p21);
+PwmOut motorLeftTwo (p22);
+
+//MD3 Arm1
+PwmOut armRight(p26);
+DigitalOut armThree (p30);
+DigitalOut armFour (p29);
+
+//MD4 Arm2
+PwmOut armLeft(p25);
+DigitalOut armOne (p6);
+DigitalOut armTwo (p5);
+
+
 /*
-PwmOut led1(LED1);
-PwmOut led2(LED2);
-DigitalOut led3(LED3);
-DigitalOut led4(LED4);
+
+PWM->0.xxx
+Digitals->0 and 0.xxx
+for arms(I guess...)
+
+For lergs, turn right or left, and just going go and back.->Probably using sticks.
+How to cha ge speed? Two different tyes of speed.
+
+Wow, Taka-chan will do one of the machine. He wants to do with the angles. Plus I don't understand what they are talking about!!
+Left stick, speed and right/left
+Push button and max incline -> the max speed
+Just max incline -> usual speed
+Little incline -> slow speed
+
+L1/R2 -> turning?
+
+Lifting ->
+
 */
 
 
 
-
-
-
-
-
-//momoi
-
-//MD1 right
-
-PwmOut motorRightOne (p23);
-PwmOut motorRightTwo (p24);
+void UserLoopSetting()
+{
+    //一度だけ行いたい初期設定をここに書く
+    /*
+    I need to ask what to do with this!
+    Actually, do we have any LEDs?
+    */
 
-//MD2 left
-PwmOut motorLeftOne (p21);
-PwmOut motorLeftTwo (p22);
-
-//MD3 Arm1
-PwmOut armOneJun(p26);
-DigitalOut chocolateOne (p30);
-DigitalOut chocolateTwo (p29);
-
-//MD4 Arm2
-PwmOut armTwoTaka(p25);
-DigitalOut brownieOne (p6);
-DigitalOut brownieTwo (p5);
-
-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;
@@ -59,7 +72,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;
@@ -70,135 +83,118 @@
         ButtonState =  ((ps3report*)(data + 1))->ButtonState;
     }
     //ここより下にプログラムを書く
-       
+
+    /*
+     double thesOne = 64;
+     double thesTwo = 127;
+     double thesThree = 191;
+     double fastSpeed = 1;
+     double slowSpeed = 0.5;
+     */
+
+//motor
+    if((ButtonState >> BUTTONSTART)&1 == 1) {
+        flag = 0;
+    } else if((ButtonState >> BUTTONR2)&1 == 1) {
+        flag += 0.05;
+        wait(0.1);
+    } else if((ButtonState >> BUTTONL2)&1 == 1) {
+        flag -= 0.05;
+        wait(0.1);
+    }
+    if(flag >= 1.0) {
+        flag = 1.0;
+    }
+    if(flag <= -0.3) {
+        flag = -0.2;
+    }
+
     if(LSY>140 && ((ButtonState >> BUTTONLANALOG)&1) == 1) {
-         motorRightOne = 0.8;
+        motorRightOne = 0.8;
         motorRightTwo = 0;
-        motorLeftOne = 0.9;
+        motorLeftOne = 0.8;
         motorLeftTwo = 0;
     } else if(LSY<110 && ((ButtonState >> BUTTONLANALOG)&1) == 1) {
-         motorRightOne = 0;
+        motorRightOne = 0;
         motorRightTwo = 0.8;
         motorLeftOne = 0;
-        motorLeftTwo = 0.9;        
-    
-     
-    } else if(LSY>140 && ((ButtonState >> BUTTONL1)&1) == 0 && ((ButtonState >> BUTTONR1)&1) == 0) {//forward
-        motorRightOne = 0.4;
-        motorRightTwo = 0;
-        motorLeftOne = 0.5;
-        motorLeftTwo = 0;
-    } else if(LSY>140 && ((ButtonState >> BUTTONL1)&1) == 1 && ((ButtonState >> BUTTONR1)&1) == 0) {//turn right
-        motorRightOne = 0;
-        motorRightTwo = 0.4;
-        motorLeftOne = 0.5;
-        motorLeftTwo = 0;
-    } else if(LSY>140 && ((ButtonState >> BUTTONL1)&1) == 0 && ((ButtonState >> BUTTONR1)&1) == 1) {//turn left
-        motorRightOne = 0.4;
-        motorRightTwo = 0;
-        motorLeftOne = 0;
-        motorLeftTwo = 0.5;
-    }else if(LSY>140 && ((ButtonState >> BUTTONL1)&1) == 1 && ((ButtonState >> BUTTONR1)&1) == 1) {//forward
-        motorRightOne = 0.4;
+        motorLeftTwo = 0.8;
+    } else if(  LSY > 140 ) { //forward
+        motorRightOne = 0.6 + flag;
         motorRightTwo = 0;
-        motorLeftOne = 0.5;
-        motorLeftTwo = 0;
-    } else if(LSY<110 && ((ButtonState >> BUTTONL1)&1) == 0 && ((ButtonState >> BUTTONR1)&1) == 0) { //back
-        motorRightOne = 0;
-        motorRightTwo = 0.4;
-        motorLeftOne = 0;
-        motorLeftTwo = 0.5;
-    } else if (LSY<110 && ((ButtonState >> BUTTONL1)&1) == 1 && ((ButtonState >> BUTTONR1)&1) == 0) { //turn right
-        motorRightOne = 0;
-        motorRightTwo = 0.4;
-        motorLeftOne = 0.5;
+        motorLeftOne = 0.6 + flag;
         motorLeftTwo = 0;
-    } else if(LSY<110 && ((ButtonState >> BUTTONL1)&1) == 0 && ((ButtonState >> BUTTONR1)&1) == 1) {//turn left
-        motorRightOne = 0.4;
-        motorRightTwo = 0;
+
+    } else if( LSY < 110 ) { //back
+        motorRightOne = 0;
+        motorRightTwo = 0.6 + flag;
         motorLeftOne = 0;
-        motorLeftTwo = 0.5;
-    }else if (LSY<110 && ((ButtonState >> BUTTONL1)&1) == 1 && ((ButtonState >> BUTTONR1)&1) == 1) {//back
-        motorRightOne = 0;
-        motorRightTwo = 0.4;
-        motorLeftOne = 0;
-        motorLeftTwo = 0.5;
-    } else if(LSY>=110 && RSY<= 140 && ((ButtonState >> BUTTONL1)&1) == 0 && ((ButtonState >> BUTTONR1)&1) == 0) {//stop
+        motorLeftTwo = 0.6 + flag;
+    } else { //stop
         motorRightOne = 0;
         motorRightTwo = 0;
         motorLeftOne = 0;
         motorLeftTwo = 0;
-    } else if(LSY>=110 && RSY<= 140 && ((ButtonState >> BUTTONL1)&1) ==1  && ((ButtonState >> BUTTONR1)&1) == 0) { //turn right
+    }
+
+    if(( ButtonState >> BUTTONR1)&1 == 1) {
         motorRightOne = 0;
-        motorRightTwo = 0.4;
-        motorLeftOne = 0.5;
+        motorRightTwo = 0.7 + flag;
+        motorLeftOne = 0.7 + flag;
         motorLeftTwo = 0;
-    } else if(LSY>=110 && RSY<= 140 && ((ButtonState >> BUTTONL1)&1) ==0  && ((ButtonState >> BUTTONR1)&1) == 1) { //turn left
-        motorRightOne = 0.4;
-        motorRightTwo = 0;
-        motorLeftOne = 0;
-        motorLeftTwo = 0.5;
-    } else if(LSY>=110 && RSY<= 140 && ((ButtonState >> BUTTONL1)&1) == 1 && ((ButtonState >> BUTTONR1)&1) == 1) { //stop
-        motorRightOne = 0;
-        motorRightTwo = 0;
-        motorLeftOne = 0;
-        motorLeftTwo = 0;
-    }else{
-        motorRightOne = 0;
+    }
+    if(( ButtonState >> BUTTONL1)&1 == 1) {
+        motorRightOne = 0.6 + flag;
         motorRightTwo = 0;
         motorLeftOne = 0;
-        motorLeftTwo = 0;
-        }
+        motorLeftTwo = 0.6 + flag;
+    }
+
+//arm
 
-    if(((ButtonState >> BUTTONDOWN)&1) == 1 && ((ButtonState >> BUTTONUP)&1) == 0) { 
-        armOneJun = 0.8;
-        chocolateOne = 1;
-        chocolateTwo = 0;
-    } else if (((ButtonState >> BUTTONDOWN)&1) == 0 && ((ButtonState >> BUTTONUP)&1) == 1) {
-        armOneJun = 0.8;
-        chocolateOne = 0;
-        chocolateTwo = 1;
-    } else {
-        armOneJun = 0;
-        chocolateOne = 0;
-        chocolateTwo = 0;
-    }
-    
-    
-    if( ((ButtonState >> BUTTONSQUARE)&1) == 1 && ((ButtonState >> BUTTONCIRCLE)&1) == 0 ) { //つかむか放す
-        armTwoTaka = 0.4;
-        brownieOne = 1;
-        brownieTwo = 0;
-    } else if ( ((ButtonState >> BUTTONSQUARE)&1) == 0 && ((ButtonState >> BUTTONCIRCLE)&1) == 1 ) {
-        armTwoTaka = 0.4;
-        brownieOne = 0;
-        brownieTwo = 1;
+    if(  (ButtonState >> BUTTONTRIANGEL)&1 == 1 ) {
+        armThree = 0;
+        armFour = 1;
+        armLeft = 0.4;
+    } else if( (ButtonState >> BUTTONCROSS)&1 == 1 ) {
+        armThree = 1;
+        armFour = 0;
+        armLeft = 0.4;
     } else {
-        armTwoTaka = 0;
-        brownieOne = 0;
-        brownieTwo = 0;
+        armThree = 0;
+        armFour = 0;
+        armLeft = 0;
+
     }
-    
-    
-    
-    
-    
-    
-    
-    
-    
-    
-    
-    
-    
-    //データ取得例
-    
-    if((ButtonState >> BUTTONCIRCLE)&1) {
-        //○が押されたとき
+
+     if((ButtonState >> BUTTONSQUARE)&1 == 1) {
+        armOne = 1;
+        armTwo = 0;
+        armRight = 0.4;
+    } else if((ButtonState >> BUTTONCIRCLE)&1 == 1) {
+        armOne = 0;
+        armTwo = 1;
+        armRight = 0.4;
+    } else {
+        armOne = 0;
+        armTwo = 0;
+        armRight = 0;
+
+        /*
+        With oen if, put both arms and legs.
+
+        Use start and select??
+
+
+        */
+
+
+
+        /*
+            led3=ButtonState & 0x0400;  //L1の状態
+            led4=ButtonState & 0x0800;  //R1の状態
+          */  //値の取得はps3.hを参照
+
     }
-   
-        
 
-   //値の取得はps3.hを参照
-        
 }
\ No newline at end of file