新部内対抗５班 / Mbed 2 deprecated 2016_Sp_kaida_taka

かいださん機体　高田くん仕様キー

Dependencies:   mbed

Fork of 2016_Spring_kaida by 新部内対抗５班

Kaida_taka.cpp@13:042b2dda2343, 2016-06-09 (annotated)

Committer:
atsushika
Date:
Thu Jun 09 11:49:20 2016 +0000
Revision:
13:042b2dda2343
????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
atsushika 13:042b2dda2343 1 #include "Utils.h"
atsushika 13:042b2dda2343 2 #include "USBHost.h"
atsushika 13:042b2dda2343 3 #include "hci.h"
atsushika 13:042b2dda2343 4 #include "ps3.h"
atsushika 13:042b2dda2343 5 #include "User.h"
atsushika 13:042b2dda2343 6
atsushika 13:042b2dda2343 7 #include "mbed.h"
atsushika 13:042b2dda2343 8
atsushika 13:042b2dda2343 9 int RSX,RSY,LSX,LSY,BSU,BSL;
atsushika 13:042b2dda2343 10 //これより下に関数外に書く要素を記入する
atsushika 13:042b2dda2343 11 /*PwmOut led1(LED1);
atsushika 13:042b2dda2343 12 PwmOut led2(LED2);
atsushika 13:042b2dda2343 13 DigitalOut led3(LED3);
atsushika 13:042b2dda2343 14 DigitalOut led4(LED4);
atsushika 13:042b2dda2343 15 */
atsushika 13:042b2dda2343 16
atsushika 13:042b2dda2343 17
atsushika 13:042b2dda2343 18
atsushika 13:042b2dda2343 19
atsushika 13:042b2dda2343 20
atsushika 13:042b2dda2343 21
atsushika 13:042b2dda2343 22 //MD1
atsushika 13:042b2dda2343 23
atsushika 13:042b2dda2343 24 PwmOut motorRightOne (p26);
atsushika 13:042b2dda2343 25 PwmOut motorRightTwo (p25);
atsushika 13:042b2dda2343 26
atsushika 13:042b2dda2343 27 //MD2
atsushika 13:042b2dda2343 28 PwmOut motorLeftOne (p23);
atsushika 13:042b2dda2343 29 PwmOut motorLeftTwo (p24);
atsushika 13:042b2dda2343 30
atsushika 13:042b2dda2343 31 //MD3 Arm1
atsushika 13:042b2dda2343 32 PwmOut armOneJun(p21);
atsushika 13:042b2dda2343 33 DigitalOut chocolateOne (p19);
atsushika 13:042b2dda2343 34 DigitalOut chocolateTwo (p20);
atsushika 13:042b2dda2343 35
atsushika 13:042b2dda2343 36 //MD4 Arm2
atsushika 13:042b2dda2343 37 PwmOut armTwoTaka(p22);
atsushika 13:042b2dda2343 38 DigitalOut brownieOne (p29);
atsushika 13:042b2dda2343 39 DigitalOut brownieTwo (p30);
atsushika 13:042b2dda2343 40
atsushika 13:042b2dda2343 41
atsushika 13:042b2dda2343 42 /*
atsushika 13:042b2dda2343 43 //momoi
atsushika 13:042b2dda2343 44
atsushika 13:042b2dda2343 45 //MD1
atsushika 13:042b2dda2343 46
atsushika 13:042b2dda2343 47 PwmOut motorRightOne (p23);
atsushika 13:042b2dda2343 48 PwmOut motorRightTwo (p24);
atsushika 13:042b2dda2343 49
atsushika 13:042b2dda2343 50 //MD2
atsushika 13:042b2dda2343 51 PwmOut motorLeftOne (p21);
atsushika 13:042b2dda2343 52 PwmOut motorLeftTwo (p22);
atsushika 13:042b2dda2343 53
atsushika 13:042b2dda2343 54 //MD3 Arm1
atsushika 13:042b2dda2343 55 PwmOut armOneJun(p26);
atsushika 13:042b2dda2343 56 DigitalOut chocolateOne (p30);
atsushika 13:042b2dda2343 57 DigitalOut chocolateTwo (p29);
atsushika 13:042b2dda2343 58
atsushika 13:042b2dda2343 59 //MD4 Arm2
atsushika 13:042b2dda2343 60 PwmOut armTwoTaka(p25);
atsushika 13:042b2dda2343 61 DigitalOut brownieOne (p6);
atsushika 13:042b2dda2343 62 DigitalOut brownieTwo (p5);
atsushika 13:042b2dda2343 63
atsushika 13:042b2dda2343 64 */
atsushika 13:042b2dda2343 65
atsushika 13:042b2dda2343 66
atsushika 13:042b2dda2343 67
atsushika 13:042b2dda2343 68
atsushika 13:042b2dda2343 69
atsushika 13:042b2dda2343 70
atsushika 13:042b2dda2343 71 /*
atsushika 13:042b2dda2343 72
atsushika 13:042b2dda2343 73 PWM->0.xxx
atsushika 13:042b2dda2343 74 Digitals->0 and 0.xxx
atsushika 13:042b2dda2343 75 for arms(I guess...)
atsushika 13:042b2dda2343 76
atsushika 13:042b2dda2343 77 For lergs, turn right or left, and just going go and back.->Probably using sticks.
atsushika 13:042b2dda2343 78 How to cha ge speed? Two different tyes of speed.
atsushika 13:042b2dda2343 79
atsushika 13:042b2dda2343 80 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!!
atsushika 13:042b2dda2343 81 Left stick, speed and right/left
atsushika 13:042b2dda2343 82 Push button and max incline -> the max speed
atsushika 13:042b2dda2343 83 Just max incline -> usual speed
atsushika 13:042b2dda2343 84 Little incline -> slow speed
atsushika 13:042b2dda2343 85
atsushika 13:042b2dda2343 86 L1/R2 -> turning?
atsushika 13:042b2dda2343 87
atsushika 13:042b2dda2343 88 Lifting ->
atsushika 13:042b2dda2343 89
atsushika 13:042b2dda2343 90 */
atsushika 13:042b2dda2343 91
atsushika 13:042b2dda2343 92
atsushika 13:042b2dda2343 93
atsushika 13:042b2dda2343 94 void UserLoopSetting()
atsushika 13:042b2dda2343 95 {
atsushika 13:042b2dda2343 96 //一度だけ行いたい初期設定をここに書く
atsushika 13:042b2dda2343 97 /*
atsushika 13:042b2dda2343 98 I need to ask what to do with this!
atsushika 13:042b2dda2343 99 Actually, do we have any LEDs?
atsushika 13:042b2dda2343 100 */
atsushika 13:042b2dda2343 101 /* led1.period_ms(20);
atsushika 13:042b2dda2343 102 led3=1;
atsushika 13:042b2dda2343 103 */
atsushika 13:042b2dda2343 104
atsushika 13:042b2dda2343 105 }
atsushika 13:042b2dda2343 106
atsushika 13:042b2dda2343 107 void UserLoop(char n,const u8* data)
atsushika 13:042b2dda2343 108 {
atsushika 13:042b2dda2343 109 u16 ButtonState;
atsushika 13:042b2dda2343 110 if(n==0) { //有線Ps3USB.cpp
atsushika 13:042b2dda2343 111 RSX = ((ps3report*)data)->RightStickX;
atsushika 13:042b2dda2343 112 RSY = ((ps3report*)data)->RightStickY;
atsushika 13:042b2dda2343 113 LSX = ((ps3report*)data)->LeftStickX;
atsushika 13:042b2dda2343 114 LSY = ((ps3report*)data)->LeftStickY;
atsushika 13:042b2dda2343 115 BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
atsushika 13:042b2dda2343 116 BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
atsushika 13:042b2dda2343 117 //ボタンの処理
atsushika 13:042b2dda2343 118 ButtonState = ((ps3report*)data)->ButtonState;
atsushika 13:042b2dda2343 119 } else {//無線TestShell.cpp
atsushika 13:042b2dda2343 120 RSX = ((ps3report*)(data + 1))->RightStickX;
atsushika 13:042b2dda2343 121 RSY = ((ps3report*)(data + 1))->RightStickY;
atsushika 13:042b2dda2343 122 LSX = ((ps3report*)(data + 1))->LeftStickX;
atsushika 13:042b2dda2343 123 LSY = ((ps3report*)(data + 1))->LeftStickY;
atsushika 13:042b2dda2343 124 BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
atsushika 13:042b2dda2343 125 BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
atsushika 13:042b2dda2343 126 //ボタンの処理
atsushika 13:042b2dda2343 127 ButtonState = ((ps3report*)(data + 1))->ButtonState;
atsushika 13:042b2dda2343 128 }
atsushika 13:042b2dda2343 129 //ここより下にプログラムを書く
atsushika 13:042b2dda2343 130
atsushika 13:042b2dda2343 131 //足回り
atsushika 13:042b2dda2343 132 /*
atsushika 13:042b2dda2343 133 double thesOne = 64;
atsushika 13:042b2dda2343 134 double thesTwo = 127;
atsushika 13:042b2dda2343 135 double thesThree = 191;
atsushika 13:042b2dda2343 136 double fastSpeed = 1;
atsushika 13:042b2dda2343 137 double slowSpeed = 0.5;
atsushika 13:042b2dda2343 138 */
atsushika 13:042b2dda2343 139
atsushika 13:042b2dda2343 140
atsushika 13:042b2dda2343 141
atsushika 13:042b2dda2343 142
atsushika 13:042b2dda2343 143
atsushika 13:042b2dda2343 144 if(LSY>140 && ((ButtonState >> BUTTONLANALOG)&1) == 1) {
atsushika 13:042b2dda2343 145 motorRightOne = 0.8;
atsushika 13:042b2dda2343 146 motorRightTwo = 0;
atsushika 13:042b2dda2343 147 motorLeftOne = 0.8;
atsushika 13:042b2dda2343 148 motorLeftTwo = 0;
atsushika 13:042b2dda2343 149 } else if(LSY<110 && ((ButtonState >> BUTTONLANALOG)&1) == 1) {
atsushika 13:042b2dda2343 150 motorRightOne = 0;
atsushika 13:042b2dda2343 151 motorRightTwo = 0.8;
atsushika 13:042b2dda2343 152 motorLeftOne = 0;
atsushika 13:042b2dda2343 153 motorLeftTwo = 0.8;
atsushika 13:042b2dda2343 154
atsushika 13:042b2dda2343 155 } else if(LSY>140 && ((ButtonState >> BUTTONL1)&1) == 0 && ((ButtonState >> BUTTONR1)&1) == 0) {//forward
atsushika 13:042b2dda2343 156 motorRightOne = 0.4;
atsushika 13:042b2dda2343 157 motorRightTwo = 0;
atsushika 13:042b2dda2343 158 motorLeftOne = 0.4;
atsushika 13:042b2dda2343 159 motorLeftTwo = 0;
atsushika 13:042b2dda2343 160 } else if(LSY>140 && ((ButtonState >> BUTTONL1)&1) == 1 && ((ButtonState >> BUTTONR1)&1) == 0) {//turn right
atsushika 13:042b2dda2343 161 motorRightOne = 0;
atsushika 13:042b2dda2343 162 motorRightTwo = 0.4;
atsushika 13:042b2dda2343 163 motorLeftOne = 0.4;
atsushika 13:042b2dda2343 164 motorLeftTwo = 0;
atsushika 13:042b2dda2343 165 } else if(LSY>140 && ((ButtonState >> BUTTONL1)&1) == 0 && ((ButtonState >> BUTTONR1)&1) == 1) {//turn left
atsushika 13:042b2dda2343 166 motorRightOne = 0.4;
atsushika 13:042b2dda2343 167 motorRightTwo = 0;
atsushika 13:042b2dda2343 168 motorLeftOne = 0;
atsushika 13:042b2dda2343 169 motorLeftTwo = 0.4;
atsushika 13:042b2dda2343 170 }else if(LSY>140 && ((ButtonState >> BUTTONL1)&1) == 1 && ((ButtonState >> BUTTONR1)&1) == 1) {//forward
atsushika 13:042b2dda2343 171 motorRightOne = 0.4;
atsushika 13:042b2dda2343 172 motorRightTwo = 0;
atsushika 13:042b2dda2343 173 motorLeftOne = 0.4;
atsushika 13:042b2dda2343 174 motorLeftTwo = 0;
atsushika 13:042b2dda2343 175 } else if(LSY<110 && ((ButtonState >> BUTTONL1)&1) == 0 && ((ButtonState >> BUTTONR1)&1) == 0) { //back
atsushika 13:042b2dda2343 176 motorRightOne = 0;
atsushika 13:042b2dda2343 177 motorRightTwo = 0.4;
atsushika 13:042b2dda2343 178 motorLeftOne = 0;
atsushika 13:042b2dda2343 179 motorLeftTwo = 0.4;
atsushika 13:042b2dda2343 180 } else if (LSY<110 && ((ButtonState >> BUTTONL1)&1) == 1 && ((ButtonState >> BUTTONR1)&1) == 0) { //turn right
atsushika 13:042b2dda2343 181 motorRightOne = 0;
atsushika 13:042b2dda2343 182 motorRightTwo = 0.4;
atsushika 13:042b2dda2343 183 motorLeftOne = 0.4;
atsushika 13:042b2dda2343 184 motorLeftTwo = 0;
atsushika 13:042b2dda2343 185 } else if(LSY<110 && ((ButtonState >> BUTTONL1)&1) == 0 && ((ButtonState >> BUTTONR1)&1) == 1) {//turn left
atsushika 13:042b2dda2343 186 motorRightOne = 0.4;
atsushika 13:042b2dda2343 187 motorRightTwo = 0;
atsushika 13:042b2dda2343 188 motorLeftOne = 0;
atsushika 13:042b2dda2343 189 motorLeftTwo = 0.4;
atsushika 13:042b2dda2343 190 }else if (LSY<110 && ((ButtonState >> BUTTONL1)&1) == 1 && ((ButtonState >> BUTTONR1)&1) == 1) {//back
atsushika 13:042b2dda2343 191 motorRightOne = 0;
atsushika 13:042b2dda2343 192 motorRightTwo = 0.4;
atsushika 13:042b2dda2343 193 motorLeftOne = 0;
atsushika 13:042b2dda2343 194 motorLeftTwo = 0.4;
atsushika 13:042b2dda2343 195 } else if(LSY>=110 && RSY<= 140 && ((ButtonState >> BUTTONL1)&1) == 0 && ((ButtonState >> BUTTONR1)&1) == 0) {//stop
atsushika 13:042b2dda2343 196 motorRightOne = 0;
atsushika 13:042b2dda2343 197 motorRightTwo = 0;
atsushika 13:042b2dda2343 198 motorLeftOne = 0;
atsushika 13:042b2dda2343 199 motorLeftTwo = 0;
atsushika 13:042b2dda2343 200 } else if(LSY>=110 && RSY<= 140 && ((ButtonState >> BUTTONL1)&1) ==1 && ((ButtonState >> BUTTONR1)&1) == 0) { //turn right
atsushika 13:042b2dda2343 201 motorRightOne = 0;
atsushika 13:042b2dda2343 202 motorRightTwo = 0.4;
atsushika 13:042b2dda2343 203 motorLeftOne = 0.4;
atsushika 13:042b2dda2343 204 motorLeftTwo = 0;
atsushika 13:042b2dda2343 205 } else if(LSY>=110 && RSY<= 140 && ((ButtonState >> BUTTONL1)&1) ==0 && ((ButtonState >> BUTTONR1)&1) == 1) { //turn left
atsushika 13:042b2dda2343 206 motorRightOne = 0.4;
atsushika 13:042b2dda2343 207 motorRightTwo = 0;
atsushika 13:042b2dda2343 208 motorLeftOne = 0;
atsushika 13:042b2dda2343 209 motorLeftTwo = 0.4;
atsushika 13:042b2dda2343 210 } else if(LSY>=110 && RSY<= 140 && ((ButtonState >> BUTTONL1)&1) == 1 && ((ButtonState >> BUTTONR1)&1) == 1) { //stop
atsushika 13:042b2dda2343 211 motorRightOne = 0;
atsushika 13:042b2dda2343 212 motorRightTwo = 0;
atsushika 13:042b2dda2343 213 motorLeftOne = 0;
atsushika 13:042b2dda2343 214 motorLeftTwo = 0;
atsushika 13:042b2dda2343 215 }else{
atsushika 13:042b2dda2343 216 motorRightOne = 0;
atsushika 13:042b2dda2343 217 motorRightTwo = 0;
atsushika 13:042b2dda2343 218 motorLeftOne = 0;
atsushika 13:042b2dda2343 219 motorLeftTwo = 0;
atsushika 13:042b2dda2343 220 }
atsushika 13:042b2dda2343 221
atsushika 13:042b2dda2343 222 if(((ButtonState >> BUTTONDOWN)&1) == 1 && ((ButtonState >> BUTTONUP)&1) == 0) {
atsushika 13:042b2dda2343 223 armOneJun = 0.4;
atsushika 13:042b2dda2343 224 chocolateOne = 1;
atsushika 13:042b2dda2343 225 chocolateTwo = 0;
atsushika 13:042b2dda2343 226 } else if (((ButtonState >> BUTTONDOWN)&1) == 0 && ((ButtonState >> BUTTONUP)&1) == 1) {
atsushika 13:042b2dda2343 227 armOneJun = 0.8;
atsushika 13:042b2dda2343 228 chocolateOne = 0;
atsushika 13:042b2dda2343 229 chocolateTwo = 1;
atsushika 13:042b2dda2343 230 } else {
atsushika 13:042b2dda2343 231 armOneJun = 0;
atsushika 13:042b2dda2343 232 chocolateOne = 0;
atsushika 13:042b2dda2343 233 chocolateTwo = 0;
atsushika 13:042b2dda2343 234 }
atsushika 13:042b2dda2343 235
atsushika 13:042b2dda2343 236
atsushika 13:042b2dda2343 237 if( ((ButtonState >> BUTTONTRIANGEL)&1) == 1 && ((ButtonState >> BUTTONCROSS)&1) == 0 ) { //つかむか放す
atsushika 13:042b2dda2343 238 armTwoTaka = 0.2;
atsushika 13:042b2dda2343 239 brownieOne = 1;
atsushika 13:042b2dda2343 240 brownieTwo = 0;
atsushika 13:042b2dda2343 241 } else if ( ((ButtonState >> BUTTONTRIANGEL)&1) == 0 && ((ButtonState >> BUTTONCROSS)&1) == 1 ) {
atsushika 13:042b2dda2343 242 armTwoTaka = 0.2;
atsushika 13:042b2dda2343 243 brownieOne = 0;
atsushika 13:042b2dda2343 244 brownieTwo = 1;
atsushika 13:042b2dda2343 245 } else {
atsushika 13:042b2dda2343 246 armTwoTaka = 0;
atsushika 13:042b2dda2343 247 brownieOne = 0;
atsushika 13:042b2dda2343 248 brownieTwo = 0;
atsushika 13:042b2dda2343 249 }
atsushika 13:042b2dda2343 250 /*
atsushika 13:042b2dda2343 251
atsushika 13:042b2dda2343 252
atsushika 13:042b2dda2343 253 if((ButtonState >> BUTTONCIRCLE)&1) {
atsushika 13:042b2dda2343 254 motorRightOne = 0.4;
atsushika 13:042b2dda2343 255 motorRightTwo = 0;
atsushika 13:042b2dda2343 256 motorLeftOne = 0.4;
atsushika 13:042b2dda2343 257 motorLeftTwo = 0;
atsushika 13:042b2dda2343 258 }else{
atsushika 13:042b2dda2343 259 motorRightOne = 0;
atsushika 13:042b2dda2343 260 motorRightTwo = 0;
atsushika 13:042b2dda2343 261 motorLeftOne = 0;
atsushika 13:042b2dda2343 262 motorLeftTwo = 0;
atsushika 13:042b2dda2343 263 }
atsushika 13:042b2dda2343 264 */
atsushika 13:042b2dda2343 265
atsushika 13:042b2dda2343 266 /*
atsushika 13:042b2dda2343 267 With oen if, put both arms and legs.
atsushika 13:042b2dda2343 268
atsushika 13:042b2dda2343 269 Use start and select??
atsushika 13:042b2dda2343 270
atsushika 13:042b2dda2343 271
atsushika 13:042b2dda2343 272 */
atsushika 13:042b2dda2343 273
atsushika 13:042b2dda2343 274 //データ取得例
atsushika 13:042b2dda2343 275
atsushika 13:042b2dda2343 276 if((ButtonState >> BUTTONCIRCLE)&1) {
atsushika 13:042b2dda2343 277 //○が押されたとき
atsushika 13:042b2dda2343 278 }
atsushika 13:042b2dda2343 279 /*
atsushika 13:042b2dda2343 280
atsushika 13:042b2dda2343 281 led1=RSX/256.0f;
atsushika 13:042b2dda2343 282 led2=RSY/256.0f;
atsushika 13:042b2dda2343 283
atsushika 13:042b2dda2343 284
atsushika 13:042b2dda2343 285 */
atsushika 13:042b2dda2343 286 /*
atsushika 13:042b2dda2343 287 led3=ButtonState & 0x0400; //L1の状態
atsushika 13:042b2dda2343 288 led4=ButtonState & 0x0800; //R1の状態
atsushika 13:042b2dda2343 289 */ //値の取得はps3.hを参照
atsushika 13:042b2dda2343 290
atsushika 13:042b2dda2343 291
atsushika 13:042b2dda2343 292
atsushika 13:042b2dda2343 293 }