momoi_taka.cpp

00001 #include "Utils.h"
00002 #include "USBHost.h"
00003 #include "hci.h"
00004 #include "ps3.h"
00005 #include "User.h"
00006 #include "mbed.h"
00007 
00008 int RSX,RSY,LSX,LSY,BSU,BSL;
00009 //これより下に関数外に書く要素を記入する
00010 double flag;
00011 //MD1
00012 PwmOut motorRightOne (p23);
00013 PwmOut motorRightTwo (p24);
00014 
00015 //MD2
00016 PwmOut motorLeftOne (p21);
00017 PwmOut motorLeftTwo (p22);
00018 
00019 //MD3 Arm1
00020 PwmOut armRight(p26);
00021 DigitalOut armThree (p30);
00022 DigitalOut armFour (p29);
00023 
00024 //MD4 Arm2
00025 PwmOut armLeft(p25);
00026 DigitalOut armOne (p6);
00027 DigitalOut armTwo (p5);
00028 
00029 
00030 /*
00031 
00032 PWM->0.xxx
00033 Digitals->0 and 0.xxx
00034 for arms(I guess...)
00035 
00036 For lergs, turn right or left, and just going go and back.->Probably using sticks.
00037 How to cha ge speed? Two different tyes of speed.
00038 
00039 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!!
00040 Left stick, speed and right/left
00041 Push button and max incline -> the max speed
00042 Just max incline -> usual speed
00043 Little incline -> slow speed
00044 
00045 L1/R2 -> turning?
00046 
00047 Lifting ->
00048 
00049 */
00050 
00051 
00052 
00053 void UserLoopSetting()
00054 {
00055     //一度だけ行いたい初期設定をここに書く
00056     /*
00057     I need to ask what to do with this!
00058     Actually, do we have any LEDs?
00059     */
00060 
00061 }
00062 
00063 void UserLoop(char n,const u8* data)
00064 {
00065     u16 ButtonState;
00066     if(n==0) { //有線Ps3USB.cpp
00067         RSX = ((ps3report*)data)->RightStickX;
00068         RSY = ((ps3report*)data)->RightStickY;
00069         LSX = ((ps3report*)data)->LeftStickX;
00070         LSY = ((ps3report*)data)->LeftStickY;
00071         BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
00072         BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
00073         //ボタンの処理
00074         ButtonState =  ((ps3report*)data)->ButtonState;
00075     } else {//無線TestShell.cpp
00076         RSX = ((ps3report*)(data + 1))->RightStickX;
00077         RSY = ((ps3report*)(data + 1))->RightStickY;
00078         LSX = ((ps3report*)(data + 1))->LeftStickX;
00079         LSY = ((ps3report*)(data + 1))->LeftStickY;
00080         BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
00081         BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
00082         //ボタンの処理
00083         ButtonState =  ((ps3report*)(data + 1))->ButtonState;
00084     }
00085     //ここより下にプログラムを書く
00086 
00087     /*
00088      double thesOne = 64;
00089      double thesTwo = 127;
00090      double thesThree = 191;
00091      double fastSpeed = 1;
00092      double slowSpeed = 0.5;
00093      */
00094 
00095 //motor
00096     if((ButtonState >> BUTTONSTART)&1 == 1) {
00097         flag = 0;
00098     } else if((ButtonState >> BUTTONR2)&1 == 1) {
00099         flag += 0.05;
00100         wait(0.1);
00101     } else if((ButtonState >> BUTTONL2)&1 == 1) {
00102         flag -= 0.05;
00103         wait(0.1);
00104     }
00105     if(flag >= 1.0) {
00106         flag = 1.0;
00107     }
00108     if(flag <= -0.3) {
00109         flag = -0.2;
00110     }
00111 
00112     if(LSY>140 && ((ButtonState >> BUTTONLANALOG)&1) == 1) {
00113         motorRightOne = 0.8;
00114         motorRightTwo = 0;
00115         motorLeftOne = 0.8;
00116         motorLeftTwo = 0;
00117     } else if(LSY<110 && ((ButtonState >> BUTTONLANALOG)&1) == 1) {
00118         motorRightOne = 0;
00119         motorRightTwo = 0.8;
00120         motorLeftOne = 0;
00121         motorLeftTwo = 0.8;
00122     } else if(  LSY > 140 ) { //forward
00123         motorRightOne = 0.6 + flag;
00124         motorRightTwo = 0;
00125         motorLeftOne = 0.6 + flag;
00126         motorLeftTwo = 0;
00127 
00128     } else if( LSY < 110 ) { //back
00129         motorRightOne = 0;
00130         motorRightTwo = 0.6 + flag;
00131         motorLeftOne = 0;
00132         motorLeftTwo = 0.6 + flag;
00133     } else { //stop
00134         motorRightOne = 0;
00135         motorRightTwo = 0;
00136         motorLeftOne = 0;
00137         motorLeftTwo = 0;
00138     }
00139 
00140     if(( ButtonState >> BUTTONR1)&1 == 1) {
00141         motorRightOne = 0;
00142         motorRightTwo = 0.7 + flag;
00143         motorLeftOne = 0.7 + flag;
00144         motorLeftTwo = 0;
00145     }
00146     if(( ButtonState >> BUTTONL1)&1 == 1) {
00147         motorRightOne = 0.6 + flag;
00148         motorRightTwo = 0;
00149         motorLeftOne = 0;
00150         motorLeftTwo = 0.6 + flag;
00151     }
00152 
00153 //arm
00154 
00155     if(  (ButtonState >> BUTTONTRIANGEL)&1 == 1 ) {
00156         armThree = 0;
00157         armFour = 1;
00158         armLeft = 0.4;
00159     } else if( (ButtonState >> BUTTONCROSS)&1 == 1 ) {
00160         armThree = 1;
00161         armFour = 0;
00162         armLeft = 0.4;
00163     } else {
00164         armThree = 0;
00165         armFour = 0;
00166         armLeft = 0;
00167 
00168     }
00169 
00170      if((ButtonState >> BUTTONSQUARE)&1 == 1) {
00171         armOne = 1;
00172         armTwo = 0;
00173         armRight = 0.4;
00174     } else if((ButtonState >> BUTTONCIRCLE)&1 == 1) {
00175         armOne = 0;
00176         armTwo = 1;
00177         armRight = 0.4;
00178     } else {
00179         armOne = 0;
00180         armTwo = 0;
00181         armRight = 0;
00182 
00183         /*
00184         With oen if, put both arms and legs.
00185 
00186         Use start and select??
00187 
00188 
00189         */
00190 
00191 
00192 
00193         /*
00194             led3=ButtonState & 0x0400;  //L1の状態
00195             led4=ButtonState & 0x0800;  //R1の状態
00196           */  //値の取得はps3.hを参照
00197 
00198     }
00199 
00200 }