keita noda / Mbed 2 deprecated 2016_Sp_momoi_0611

ももいくん

Dependencies:   mbed

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

momoi_taka.cpp@13:9e04b53e875a, 2016-06-10 (annotated)

Committer:
abcdefgh
Date:
Fri Jun 10 17:27:13 2016 +0000
Revision:
13:9e04b53e875a
Parent:
12:d811e29e469a 
2016/06/10

Who changed what in which revision?

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