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

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

Dependencies:   mbed

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

KaidaKari.cpp@12:b2f3c85cba36, 2016-06-04 (annotated)

Committer:
Junbug
Date:
Sat Jun 04 09:06:29 2016 +0000
Revision:
12:b2f3c85cba36
2016/06/04 18:06

Who changed what in which revision?

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