2015 robotic contest arakawa A / Mbed 2 deprecated Nucleo_PS3_Jikken_PS3

マザー 20151028 PS3

Dependencies:   mbed

Fork of Nucleo_PS3_Jikken3 by 2015 robotic contest arakawa A

main.cpp@17:1942c2ba89a0, 2015-10-07 (annotated)

Committer:
nodoame
Date:
Wed Oct 07 10:49:50 2015 +0000
Revision:
17:1942c2ba89a0
Parent:
16:457bb8cc409b
Child:
18:e3591b0b9e40
?????????; 20151007

Who changed what in which revision?

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