マザー 20151028 XBOX-XBEE
Dependencies: mbed
Fork of Nucleo_PS3_Jikken_XBOX by
main.cpp@15:c941d06092ec, 2015-10-05 (annotated)
- Committer:
- nodoame
- Date:
- Mon Oct 05 08:12:15 2015 +0000
- Revision:
- 15:c941d06092ec
- Parent:
- 14:aaf2ae25e508
- Child:
- 16:457bb8cc409b
20151005;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nodoame | 0:dc587be179e9 | 1 | #include "mbed.h" |
nodoame | 0:dc587be179e9 | 2 | #include "math.h" |
nodoame | 0:dc587be179e9 | 3 | #define M_PI 3.1415926535897932384626433832795 |
nodoame | 0:dc587be179e9 | 4 | #define trigL 0 |
nodoame | 0:dc587be179e9 | 5 | #define trigR 1 |
nodoame | 0:dc587be179e9 | 6 | #define btL 2 |
nodoame | 0:dc587be179e9 | 7 | #define btR 3 |
nodoame | 0:dc587be179e9 | 8 | #define btTri 4 |
nodoame | 0:dc587be179e9 | 9 | #define btSph 5 |
nodoame | 0:dc587be179e9 | 10 | #define btCro 6 |
nodoame | 0:dc587be179e9 | 11 | #define btSqa 7 |
nodoame | 5:428b7ac86810 | 12 | #define presUp 0 |
nodoame | 5:428b7ac86810 | 13 | #define presRight 1 |
nodoame | 5:428b7ac86810 | 14 | #define presDown 2 |
nodoame | 5:428b7ac86810 | 15 | #define presLeft 3 |
nodoame | 4:bae4e2d103d4 | 16 | #define devideNum 32 |
nodoame | 4:bae4e2d103d4 | 17 | #define ED 132 |
nodoame | 6:5171a8245b57 | 18 | #define TD 144//208 |
nodoame | 5:428b7ac86810 | 19 | #define EL 134 |
nodoame | 5:428b7ac86810 | 20 | #define TL 200 |
nodoame | 6:5171a8245b57 | 21 | #define TNR 138//204 |
nodoame | 6:5171a8245b57 | 22 | #define TNL 135 |
nodoame | 11:1ca2498815c9 | 23 | #define debugFlag 1 |
nodoame | 11:1ca2498815c9 | 24 | #define stopCount 33 |
nodoame | 0:dc587be179e9 | 25 | |
nodoame | 0:dc587be179e9 | 26 | Serial pc(SERIAL_TX, SERIAL_RX); |
nodoame | 1:a3bfd422ea9e | 27 | //Serial Dev(D8,D2);//コントローラー |
nodoame | 3:c290afbacd1c | 28 | Serial Dev(PC_6,PA_12);//コントローラー |
nodoame | 1:a3bfd422ea9e | 29 | RawSerial Mechanum(D8,D2);//メカナム |
nodoame | 0:dc587be179e9 | 30 | I2C i2c(D14,D15);//sda,scl |
nodoame | 5:428b7ac86810 | 31 | //const int airAddr=0x20; |
nodoame | 0:dc587be179e9 | 32 | const int airAddr=0xA0; |
nodoame | 5:428b7ac86810 | 33 | const int valveAddr=0xC0; |
nodoame | 1:a3bfd422ea9e | 34 | BusOut dI2c(PC_0,PC_1,PC_2,PC_3);//i2cデバッグ |
nodoame | 0:dc587be179e9 | 35 | |
nodoame | 0:dc587be179e9 | 36 | int Bflag = 0; |
nodoame | 0:dc587be179e9 | 37 | DigitalOut myled(LED1);// ,test1(PB_7) ,test2(PA_15) ,test3(PC_13) ,test4(PC_12); |
nodoame | 4:bae4e2d103d4 | 38 | PwmOut Blue(D5),Green(D4),Red(D3); |
nodoame | 4:bae4e2d103d4 | 39 | //PwmOut kRed(A3),kBlue(A1),kGreen(A0); |
nodoame | 4:bae4e2d103d4 | 40 | PwmOut kRed(A3),kBlue(PA_15),kGreen(PB_7); |
nodoame | 5:428b7ac86810 | 41 | #define N 7 |
nodoame | 0:dc587be179e9 | 42 | char DATA[N]; |
nodoame | 0:dc587be179e9 | 43 | void rotate(double setAngle); |
nodoame | 0:dc587be179e9 | 44 | void speed(); |
nodoame | 11:1ca2498815c9 | 45 | int stopCounter=0; |
nodoame | 1:a3bfd422ea9e | 46 | |
nodoame | 5:428b7ac86810 | 47 | void num()//割り込まれたら7回受信 |
nodoame | 0:dc587be179e9 | 48 | { |
nodoame | 0:dc587be179e9 | 49 | if(Dev.getc()==114){ |
nodoame | 0:dc587be179e9 | 50 | //printf("ReceiveCommand\r\n"); |
nodoame | 0:dc587be179e9 | 51 | for(int i = 0 ;i<N ;i++ ) |
nodoame | 0:dc587be179e9 | 52 | { |
nodoame | 0:dc587be179e9 | 53 | DATA[i]=Dev.getc(); |
nodoame | 0:dc587be179e9 | 54 | } |
nodoame | 0:dc587be179e9 | 55 | } |
nodoame | 11:1ca2498815c9 | 56 | stopCounter=0; |
nodoame | 0:dc587be179e9 | 57 | } |
nodoame | 0:dc587be179e9 | 58 | |
nodoame | 0:dc587be179e9 | 59 | bool getBt(int num) |
nodoame | 0:dc587be179e9 | 60 | { |
nodoame | 0:dc587be179e9 | 61 | return (DATA[0]>>num)%2; |
nodoame | 0:dc587be179e9 | 62 | } |
nodoame | 0:dc587be179e9 | 63 | |
nodoame | 5:428b7ac86810 | 64 | bool getPress(int num) |
nodoame | 5:428b7ac86810 | 65 | { |
nodoame | 5:428b7ac86810 | 66 | return (DATA[6]>>num)%2; |
nodoame | 5:428b7ac86810 | 67 | } |
nodoame | 5:428b7ac86810 | 68 | |
nodoame | 0:dc587be179e9 | 69 | double pointToDeg(double y,double x,int threshold)//座標から角度を求める関数thresholdはしきい値 |
nodoame | 0:dc587be179e9 | 70 | { |
nodoame | 0:dc587be179e9 | 71 | if(abs((int)(y-128))<threshold) |
nodoame | 0:dc587be179e9 | 72 | y=128; |
nodoame | 0:dc587be179e9 | 73 | if(abs((int)(x-128))<threshold) |
nodoame | 0:dc587be179e9 | 74 | x=128; |
nodoame | 0:dc587be179e9 | 75 | return atan2(-(y-128),x-128)/M_PI*180;//y軸反転、/PI*180でRadからDeg変換 |
nodoame | 0:dc587be179e9 | 76 | } |
nodoame | 0:dc587be179e9 | 77 | |
nodoame | 0:dc587be179e9 | 78 | double devidePoint(double y,double x,int threshold,float devide) |
nodoame | 0:dc587be179e9 | 79 | { |
nodoame | 0:dc587be179e9 | 80 | double tempX,tempY,dev;//仮のX,Yと分けた角度 |
nodoame | 0:dc587be179e9 | 81 | tempX=(x-128);tempY=(y-128);//それぞれ-128 |
nodoame | 0:dc587be179e9 | 82 | if(abs((int)tempY)<threshold)//しきい値以下なら0にする |
nodoame | 0:dc587be179e9 | 83 | tempY=0; |
nodoame | 0:dc587be179e9 | 84 | if(abs((int)tempX)<threshold) |
nodoame | 0:dc587be179e9 | 85 | tempX=0; |
nodoame | 0:dc587be179e9 | 86 | if(tempX==0&&tempY==0) |
nodoame | 0:dc587be179e9 | 87 | dev=360; |
nodoame | 0:dc587be179e9 | 88 | //printf("X:%lfY:%lfDev:%lf\r\n",tempX,tempY,dev); |
nodoame | 0:dc587be179e9 | 89 | else |
nodoame | 0:dc587be179e9 | 90 | dev=atan2(tempX,-tempY)/M_PI*180;//軸を逆にして代入、Y軸反転 |
nodoame | 0:dc587be179e9 | 91 | return ((dev<0)?(dev+360):(dev))/devide;//分割 |
nodoame | 0:dc587be179e9 | 92 | } |
nodoame | 0:dc587be179e9 | 93 | |
nodoame | 4:bae4e2d103d4 | 94 | int valueForMechanum(int deg,int devide,bool triL,bool triR,bool btSpd) |
nodoame | 0:dc587be179e9 | 95 | { |
nodoame | 0:dc587be179e9 | 96 | bool stpFlag=false,spnFlag=false;//ストップフラグ、スピンフラグ |
nodoame | 0:dc587be179e9 | 97 | int val=0,i;//値、For用 |
nodoame | 0:dc587be179e9 | 98 | for(i=0;i<6;i++){ |
nodoame | 0:dc587be179e9 | 99 | switch(i){ |
nodoame | 0:dc587be179e9 | 100 | case 0: |
nodoame | 4:bae4e2d103d4 | 101 | if(deg!=devide||(triL||triR))//スピンかニュートラル以外なら |
nodoame | 0:dc587be179e9 | 102 | val=1; |
nodoame | 0:dc587be179e9 | 103 | else |
nodoame | 0:dc587be179e9 | 104 | stpFlag=true;//ストップ |
nodoame | 0:dc587be179e9 | 105 | break; |
nodoame | 0:dc587be179e9 | 106 | |
nodoame | 0:dc587be179e9 | 107 | case 1: |
nodoame | 0:dc587be179e9 | 108 | if(stpFlag) |
nodoame | 4:bae4e2d103d4 | 109 | val++; |
nodoame | 0:dc587be179e9 | 110 | else |
nodoame | 0:dc587be179e9 | 111 | val+=btSpd;//スピード |
nodoame | 0:dc587be179e9 | 112 | break; |
nodoame | 0:dc587be179e9 | 113 | |
nodoame | 0:dc587be179e9 | 114 | case 2: |
nodoame | 0:dc587be179e9 | 115 | if(triL||triR){//トリガーのどちらかがONなら |
nodoame | 0:dc587be179e9 | 116 | val++; |
nodoame | 0:dc587be179e9 | 117 | spnFlag=true;//スピンフラグON |
nodoame | 0:dc587be179e9 | 118 | } |
nodoame | 0:dc587be179e9 | 119 | break; |
nodoame | 0:dc587be179e9 | 120 | |
nodoame | 0:dc587be179e9 | 121 | case 5: |
nodoame | 4:bae4e2d103d4 | 122 | val=val<<2;//5bit目から7bit目へ |
nodoame | 0:dc587be179e9 | 123 | if(spnFlag)//トリガーのどちらかの値 |
nodoame | 0:dc587be179e9 | 124 | val+=(triL)?1:0; |
nodoame | 4:bae4e2d103d4 | 125 | else if(!stpFlag)//ジョイスティックの値 |
nodoame | 0:dc587be179e9 | 126 | val+=deg; |
nodoame | 0:dc587be179e9 | 127 | break; |
nodoame | 0:dc587be179e9 | 128 | } |
nodoame | 0:dc587be179e9 | 129 | if(i<5) |
nodoame | 0:dc587be179e9 | 130 | val=val<<1; |
nodoame | 0:dc587be179e9 | 131 | } |
nodoame | 0:dc587be179e9 | 132 | //printf("val:%ddeg:%dtrigL:%dtrigR:%dbtSp:%d\r\n",val,deg,triL,triR,btSpd); |
nodoame | 0:dc587be179e9 | 133 | return val; |
nodoame | 0:dc587be179e9 | 134 | } |
nodoame | 0:dc587be179e9 | 135 | |
nodoame | 15:c941d06092ec | 136 | void detectPole(int scVector) |
nodoame | 0:dc587be179e9 | 137 | { |
nodoame | 15:c941d06092ec | 138 | int vector=TD,laser=0; |
nodoame | 15:c941d06092ec | 139 | bool btFlag=false; |
nodoame | 4:bae4e2d103d4 | 140 | Blue=0.0f;Green=0.0f,Red=0.8f; |
nodoame | 0:dc587be179e9 | 141 | while(true) |
nodoame | 0:dc587be179e9 | 142 | { |
nodoame | 14:aaf2ae25e508 | 143 | //スイッチが押された時 |
nodoame | 15:c941d06092ec | 144 | if((int)DATA[1]&12==12&&!btFlag){ |
nodoame | 5:428b7ac86810 | 145 | wait(0.001f); |
nodoame | 5:428b7ac86810 | 146 | Mechanum.putc(64); |
nodoame | 4:bae4e2d103d4 | 147 | btFlag=true; |
nodoame | 4:bae4e2d103d4 | 148 | Blue=0.0f;Green=0.8f;Red=0; |
nodoame | 15:c941d06092ec | 149 | vector=scVector; |
nodoame | 14:aaf2ae25e508 | 150 | } |
nodoame | 11:1ca2498815c9 | 151 | |
nodoame | 15:c941d06092ec | 152 | //片方のレーザーが反応したとき |
nodoame | 15:c941d06092ec | 153 | if(btFlag){ |
nodoame | 15:c941d06092ec | 154 | if(!(int)DATA[1]&2){ |
nodoame | 15:c941d06092ec | 155 | laser=1; |
nodoame | 15:c941d06092ec | 156 | vector=0; |
nodoame | 15:c941d06092ec | 157 | } |
nodoame | 15:c941d06092ec | 158 | else if((int)DATA[1]&1){ |
nodoame | 15:c941d06092ec | 159 | laser=2; |
nodoame | 15:c941d06092ec | 160 | vector=0; |
nodoame | 9:489a085d1703 | 161 | } |
nodoame | 9:489a085d1703 | 162 | } |
nodoame | 15:c941d06092ec | 163 | |
nodoame | 15:c941d06092ec | 164 | //もう片方のレーザーが反応した時 |
nodoame | 15:c941d06092ec | 165 | else if(!(int)DATA[1]&laser&&laser){ |
nodoame | 15:c941d06092ec | 166 | Mechanum.putc(64); |
nodoame | 0:dc587be179e9 | 167 | break; |
nodoame | 0:dc587be179e9 | 168 | } |
nodoame | 4:bae4e2d103d4 | 169 | |
nodoame | 15:c941d06092ec | 170 | //pc.printf("%d\r\n",DATA[1]); |
nodoame | 0:dc587be179e9 | 171 | Mechanum.putc(vector); |
nodoame | 0:dc587be179e9 | 172 | wait(0.001f); |
nodoame | 0:dc587be179e9 | 173 | } |
nodoame | 4:bae4e2d103d4 | 174 | btFlag=false; |
nodoame | 5:428b7ac86810 | 175 | Blue=0;Green=0;Red=0; |
nodoame | 0:dc587be179e9 | 176 | } |
nodoame | 0:dc587be179e9 | 177 | |
nodoame | 0:dc587be179e9 | 178 | |
nodoame | 5:428b7ac86810 | 179 | bool shotToDenziben(char reg, char data ,int size,int addr) |
nodoame | 0:dc587be179e9 | 180 | { |
nodoame | 5:428b7ac86810 | 181 | char REG[2] = {reg,size}; |
nodoame | 5:428b7ac86810 | 182 | char DATA[2]={data,0}; |
nodoame | 5:428b7ac86810 | 183 | bool A = i2c.write(addr,REG,2); |
nodoame | 5:428b7ac86810 | 184 | A|= i2c.write(addr,DATA,size); |
nodoame | 1:a3bfd422ea9e | 185 | return A; |
nodoame | 0:dc587be179e9 | 186 | } |
nodoame | 0:dc587be179e9 | 187 | |
nodoame | 5:428b7ac86810 | 188 | char getshot(char reg) |
nodoame | 5:428b7ac86810 | 189 | { |
nodoame | 5:428b7ac86810 | 190 | char input=0; |
nodoame | 5:428b7ac86810 | 191 | char DATA[2] = {reg,1}; |
nodoame | 5:428b7ac86810 | 192 | i2c.write(airAddr,DATA,2); |
nodoame | 5:428b7ac86810 | 193 | wait(0.005f); |
nodoame | 5:428b7ac86810 | 194 | i2c.read(airAddr,&input,1); |
nodoame | 5:428b7ac86810 | 195 | return input; |
nodoame | 5:428b7ac86810 | 196 | } |
nodoame | 5:428b7ac86810 | 197 | |
nodoame | 12:6445a3c3d34c | 198 | void setFCLED(double RED,double GREEN,double BLUE) |
nodoame | 12:6445a3c3d34c | 199 | { |
nodoame | 12:6445a3c3d34c | 200 | Red=RED; |
nodoame | 12:6445a3c3d34c | 201 | Green=GREEN; |
nodoame | 12:6445a3c3d34c | 202 | Blue=BLUE; |
nodoame | 12:6445a3c3d34c | 203 | } |
nodoame | 12:6445a3c3d34c | 204 | |
nodoame | 4:bae4e2d103d4 | 205 | int main() { |
nodoame | 0:dc587be179e9 | 206 | pc.baud(230400); |
nodoame | 0:dc587be179e9 | 207 | Dev.baud(921600); |
nodoame | 0:dc587be179e9 | 208 | Mechanum.baud(230400); |
nodoame | 5:428b7ac86810 | 209 | DigitalIn bt(USER_BUTTON); |
nodoame | 0:dc587be179e9 | 210 | Dev.attach(num,Serial::RxIrq);//受信割り込み設定 |
nodoame | 1:a3bfd422ea9e | 211 | Green=0;Blue=0;Red=0; |
nodoame | 12:6445a3c3d34c | 212 | double gValue=0,bValue=0,rValue=0; |
nodoame | 3:c290afbacd1c | 213 | DigitalOut l(PB_2); |
nodoame | 3:c290afbacd1c | 214 | int sqf=0; |
nodoame | 0:dc587be179e9 | 215 | double deg; |
nodoame | 0:dc587be179e9 | 216 | int val; |
nodoame | 1:a3bfd422ea9e | 217 | i2c.frequency(400000); |
nodoame | 12:6445a3c3d34c | 218 | setFCLED(0,0.8,0.8); |
nodoame | 5:428b7ac86810 | 219 | bool obonFlag=false,pLeftF=false,pRightF=false,btCrF=false; |
nodoame | 0:dc587be179e9 | 220 | |
nodoame | 0:dc587be179e9 | 221 | while(true) |
nodoame | 12:6445a3c3d34c | 222 | { |
nodoame | 5:428b7ac86810 | 223 | //ポール検出モード |
nodoame | 0:dc587be179e9 | 224 | if(getBt(btTri)&&getBt(btSph)){ |
nodoame | 15:c941d06092ec | 225 | if(deg<17) |
nodoame | 15:c941d06092ec | 226 | detectPole(TNL); |
nodoame | 15:c941d06092ec | 227 | else |
nodoame | 15:c941d06092ec | 228 | detectPole(TNR); |
nodoame | 0:dc587be179e9 | 229 | //printf("DetectMode\r\n"); |
nodoame | 0:dc587be179e9 | 230 | } |
nodoame | 3:c290afbacd1c | 231 | |
nodoame | 5:428b7ac86810 | 232 | //射出 |
nodoame | 5:428b7ac86810 | 233 | if(getBt(btL)&&obonFlag){ |
nodoame | 5:428b7ac86810 | 234 | kRed=shotToDenziben(0x02,39,1,airAddr); |
nodoame | 5:428b7ac86810 | 235 | wait(0.1f); |
nodoame | 5:428b7ac86810 | 236 | kRed=0; |
nodoame | 5:428b7ac86810 | 237 | } |
nodoame | 5:428b7ac86810 | 238 | |
nodoame | 5:428b7ac86810 | 239 | //射出 |
nodoame | 5:428b7ac86810 | 240 | if(getBt(btR)&&obonFlag){ |
nodoame | 5:428b7ac86810 | 241 | kRed=shotToDenziben(0x02,23,1,airAddr); |
nodoame | 5:428b7ac86810 | 242 | wait(0.1f); |
nodoame | 3:c290afbacd1c | 243 | //printf("shot:%d\r\n",i2cS=i2c.write(airAddr,cmd,2)); |
nodoame | 3:c290afbacd1c | 244 | //i2c.write(airAddr,cmd,2); |
nodoame | 4:bae4e2d103d4 | 245 | kRed=0; |
nodoame | 3:c290afbacd1c | 246 | } |
nodoame | 3:c290afbacd1c | 247 | |
nodoame | 5:428b7ac86810 | 248 | //射出 |
nodoame | 5:428b7ac86810 | 249 | if(getBt(btTri)&&obonFlag){ |
nodoame | 5:428b7ac86810 | 250 | kRed=shotToDenziben(0x02,15,1,airAddr); |
nodoame | 12:6445a3c3d34c | 251 | wait(0.2f); |
nodoame | 12:6445a3c3d34c | 252 | kRed=shotToDenziben(0x02,0,1,airAddr); |
nodoame | 4:bae4e2d103d4 | 253 | kRed=0; |
nodoame | 3:c290afbacd1c | 254 | } |
nodoame | 3:c290afbacd1c | 255 | |
nodoame | 5:428b7ac86810 | 256 | //バルブ開放 |
nodoame | 5:428b7ac86810 | 257 | if(getPress(presLeft)&&pLeftF){ |
nodoame | 5:428b7ac86810 | 258 | pLeftF=false; |
nodoame | 5:428b7ac86810 | 259 | kRed=shotToDenziben(7,1,1,valveAddr); |
nodoame | 14:aaf2ae25e508 | 260 | wait(2.0); |
nodoame | 14:aaf2ae25e508 | 261 | if(!kRed) |
nodoame | 14:aaf2ae25e508 | 262 | kRed=shotToDenziben(7,0,1,valveAddr); |
nodoame | 5:428b7ac86810 | 263 | } |
nodoame | 14:aaf2ae25e508 | 264 | else if(!getPress(presLeft)){ |
nodoame | 14:aaf2ae25e508 | 265 | kRed=0; |
nodoame | 5:428b7ac86810 | 266 | pLeftF=true; |
nodoame | 14:aaf2ae25e508 | 267 | } |
nodoame | 5:428b7ac86810 | 268 | |
nodoame | 5:428b7ac86810 | 269 | //バルブ開放 |
nodoame | 5:428b7ac86810 | 270 | if(getPress(presRight)&&pRightF){ |
nodoame | 5:428b7ac86810 | 271 | pRightF=false; |
nodoame | 5:428b7ac86810 | 272 | kRed=shotToDenziben(8,1,1,valveAddr); |
nodoame | 14:aaf2ae25e508 | 273 | wait(2.0); |
nodoame | 14:aaf2ae25e508 | 274 | if(!kRed) |
nodoame | 14:aaf2ae25e508 | 275 | kRed=shotToDenziben(8,0,1,valveAddr); |
nodoame | 0:dc587be179e9 | 276 | } |
nodoame | 14:aaf2ae25e508 | 277 | else if(!getPress(presRight)){ |
nodoame | 14:aaf2ae25e508 | 278 | kRed=0; |
nodoame | 5:428b7ac86810 | 279 | pRightF=true; |
nodoame | 14:aaf2ae25e508 | 280 | } |
nodoame | 0:dc587be179e9 | 281 | |
nodoame | 5:428b7ac86810 | 282 | //バルブ開放 |
nodoame | 5:428b7ac86810 | 283 | if(getBt(btCro)&&btCrF){ |
nodoame | 5:428b7ac86810 | 284 | btCrF=false; |
nodoame | 5:428b7ac86810 | 285 | kRed=shotToDenziben(9,1,1,valveAddr); |
nodoame | 14:aaf2ae25e508 | 286 | wait(0.2); |
nodoame | 14:aaf2ae25e508 | 287 | if(!kRed) |
nodoame | 14:aaf2ae25e508 | 288 | kRed=shotToDenziben(9,0,1,valveAddr); |
nodoame | 5:428b7ac86810 | 289 | } |
nodoame | 5:428b7ac86810 | 290 | else if(!getBt(btCro)) |
nodoame | 14:aaf2ae25e508 | 291 | { |
nodoame | 14:aaf2ae25e508 | 292 | kRed=0; |
nodoame | 5:428b7ac86810 | 293 | btCrF=true; |
nodoame | 14:aaf2ae25e508 | 294 | } |
nodoame | 5:428b7ac86810 | 295 | |
nodoame | 5:428b7ac86810 | 296 | |
nodoame | 5:428b7ac86810 | 297 | //レーザーポインタ |
nodoame | 3:c290afbacd1c | 298 | if(getBt(btSqa)&&sqf){ |
nodoame | 3:c290afbacd1c | 299 | sqf=0; |
nodoame | 3:c290afbacd1c | 300 | l=!l; |
nodoame | 4:bae4e2d103d4 | 301 | kBlue=!kBlue; |
nodoame | 0:dc587be179e9 | 302 | } |
nodoame | 3:c290afbacd1c | 303 | else if(!getBt(btSqa)) |
nodoame | 3:c290afbacd1c | 304 | sqf=1; |
nodoame | 3:c290afbacd1c | 305 | |
nodoame | 5:428b7ac86810 | 306 | //タッチセンサ |
nodoame | 12:6445a3c3d34c | 307 | if(((int)DATA[1]&12)==12){ |
nodoame | 12:6445a3c3d34c | 308 | bValue=0.0f;gValue=0.8f;rValue=0; |
nodoame | 12:6445a3c3d34c | 309 | } |
nodoame | 12:6445a3c3d34c | 310 | else{ |
nodoame | 12:6445a3c3d34c | 311 | bValue=0.8f;gValue=0;rValue=0; |
nodoame | 12:6445a3c3d34c | 312 | } |
nodoame | 4:bae4e2d103d4 | 313 | |
nodoame | 5:428b7ac86810 | 314 | //お盆回し |
nodoame | 5:428b7ac86810 | 315 | if(getPress(presUp)){ |
nodoame | 5:428b7ac86810 | 316 | kRed=shotToDenziben(0x02,7,1,airAddr); |
nodoame | 5:428b7ac86810 | 317 | wait(0.1); |
nodoame | 5:428b7ac86810 | 318 | obonFlag=true; |
nodoame | 12:6445a3c3d34c | 319 | rValue=0.8;gValue=0;bValue=0.8; |
nodoame | 5:428b7ac86810 | 320 | kRed=0; |
nodoame | 5:428b7ac86810 | 321 | } |
nodoame | 5:428b7ac86810 | 322 | else if(getPress(presDown)){ |
nodoame | 5:428b7ac86810 | 323 | kRed=shotToDenziben(0x02,0,1,airAddr); |
nodoame | 5:428b7ac86810 | 324 | wait(0.1); |
nodoame | 12:6445a3c3d34c | 325 | bValue=0.8f;gValue=0;rValue=0; |
nodoame | 5:428b7ac86810 | 326 | obonFlag=false; |
nodoame | 5:428b7ac86810 | 327 | kRed=0; |
nodoame | 5:428b7ac86810 | 328 | } |
nodoame | 5:428b7ac86810 | 329 | |
nodoame | 4:bae4e2d103d4 | 330 | deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum); |
nodoame | 4:bae4e2d103d4 | 331 | val=valueForMechanum(deg,devideNum,getBt(trigL),getBt(trigR),getBt(btSph)); |
nodoame | 11:1ca2498815c9 | 332 | |
nodoame | 11:1ca2498815c9 | 333 | //一定時間通信してないと停止 |
nodoame | 11:1ca2498815c9 | 334 | if(stopCounter>stopCount) |
nodoame | 11:1ca2498815c9 | 335 | val=64; |
nodoame | 11:1ca2498815c9 | 336 | else |
nodoame | 11:1ca2498815c9 | 337 | stopCounter++; |
nodoame | 0:dc587be179e9 | 338 | Mechanum.putc(val); |
nodoame | 12:6445a3c3d34c | 339 | setFCLED(rValue,gValue,bValue); |
nodoame | 0:dc587be179e9 | 340 | wait(0.03f); |
nodoame | 11:1ca2498815c9 | 341 | do{ |
nodoame | 11:1ca2498815c9 | 342 | if(!debugFlag) |
nodoame | 11:1ca2498815c9 | 343 | break; |
nodoame | 14:aaf2ae25e508 | 344 | //pc.printf("val:%d\r\n",val); |
nodoame | 11:1ca2498815c9 | 345 | //printf("%lf:%d:%d\r\n",deg,(int)DATA[3],(int)DATA[2]); |
nodoame | 11:1ca2498815c9 | 346 | //printf("%d\r\n",(int)DATA[0]); |
nodoame | 11:1ca2498815c9 | 347 | //pc.printf("DATA[1]%d\r\n",(int)(DATA[1]&0x02)); |
nodoame | 11:1ca2498815c9 | 348 | //printf("deg:%lfX1:%d:bt%ddeg:%lfval:%d\r\n",deg,(int)DATA[1],getBt(btSqa),deg,val); |
nodoame | 11:1ca2498815c9 | 349 | }while(false); |
nodoame | 0:dc587be179e9 | 350 | } |
nodoame | 0:dc587be179e9 | 351 | } |