2015 robotic contest arakawa A / Mbed 2 deprecated Nucleo_PS3_Jikken_XBOX

マザー 20151028 XBOX

Dependencies:   mbed

Fork of Nucleo_PS3_Jikken_PS3 by 2015 robotic contest arakawa A

main.cpp@19:cd036cc40a2d, 2015-10-13 (annotated)

Committer:
nodoame
Date:
Tue Oct 13 07:40:34 2015 +0000
Revision:
19:cd036cc40a2d
Parent:
18:e3591b0b9e40
Child:
20:373fa71ce05b
20151013; ????

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 19:cd036cc40a2d 12 #define presUp 0
nodoame 19:cd036cc40a2d 13 #define presRight 1
nodoame 19:cd036cc40a2d 14 #define presDown 2
nodoame 19:cd036cc40a2d 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 19:cd036cc40a2d 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 19:cd036cc40a2d 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 19:cd036cc40a2d 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)//しきい値以下なら０にする
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 19:cd036cc40a2d 136 void detectPole(int scVector)
nodoame 0:dc587be179e9 137 {
nodoame 15:c941d06092ec 138 int vector=TD,laser=0;
nodoame 19:cd036cc40a2d 139 bool btFlag=false;
nodoame 4:bae4e2d103d4 140 Blue=0.0f;Green=0.0f,Red=0.8f;
nodoame 0:dc587be179e9 141 while(true)
nodoame 19:cd036cc40a2d 142 {
nodoame 14:aaf2ae25e508 143 //スイッチが押された時
nodoame 19:cd036cc40a2d 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 19:cd036cc40a2d 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 19:cd036cc40a2d 153 if(btFlag){
nodoame 19:cd036cc40a2d 154 if(!(int)DATA[1]&2){
nodoame 15:c941d06092ec 155 laser=1;
nodoame 15:c941d06092ec 156 vector=0;
nodoame 15:c941d06092ec 157 }
nodoame 19:cd036cc40a2d 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 19:cd036cc40a2d 165 else if(!(int)DATA[1]&laser&&laser){
nodoame 19:cd036cc40a2d 166 Mechanum.putc(64);
nodoame 19:cd036cc40a2d 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 19:cd036cc40a2d 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 19:cd036cc40a2d 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 19:cd036cc40a2d 219 bool obonFlag=false,pLeftF=false,pRightF=false,btCrF=false,pUpF=false,pDownF=false,btLF=false,btRF=false,btTrF=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 19:cd036cc40a2d 225 if(deg<17)
nodoame 19:cd036cc40a2d 226 detectPole(TNL);
nodoame 19:cd036cc40a2d 227 else
nodoame 19:cd036cc40a2d 228 detectPole(TNR);
nodoame 19:cd036cc40a2d 229 //printf("DetectMode\r\n");
nodoame 0:dc587be179e9 230 }
nodoame 3:c290afbacd1c 231
nodoame 5:428b7ac86810 232 //射出
nodoame 19:cd036cc40a2d 233 if(getBt(btL)&&obonFlag&&btLF){
nodoame 19:cd036cc40a2d 234 kRed=shotToDenziben(0x02,39,1,airAddr);
nodoame 19:cd036cc40a2d 235 wait(0.1f);
nodoame 19:cd036cc40a2d 236 }
nodoame 19:cd036cc40a2d 237 else if(!getBt(btL)){
nodoame 19:cd036cc40a2d 238 btLF=true;
nodoame 19:cd036cc40a2d 239 kRed=0;
nodoame 19:cd036cc40a2d 240 }
nodoame 19:cd036cc40a2d 241
nodoame 19:cd036cc40a2d 242
nodoame 19:cd036cc40a2d 243 //射出
nodoame 19:cd036cc40a2d 244 if(getBt(btR)&&obonFlag&&btRF){
nodoame 19:cd036cc40a2d 245 kRed=shotToDenziben(0x02,23,1,airAddr);
nodoame 19:cd036cc40a2d 246 wait(0.1f);
nodoame 19:cd036cc40a2d 247 //printf("shot:%d\r\n",i2cS=i2c.write(airAddr,cmd,2));
nodoame 19:cd036cc40a2d 248 //i2c.write(airAddr,cmd,2);
nodoame 19:cd036cc40a2d 249 }
nodoame 19:cd036cc40a2d 250 else if(!getBt(btR)){
nodoame 19:cd036cc40a2d 251 btRF=true;
nodoame 5:428b7ac86810 252 kRed=0;
nodoame 5:428b7ac86810 253 }
nodoame 5:428b7ac86810 254
nodoame 5:428b7ac86810 255 //射出
nodoame 19:cd036cc40a2d 256 if(getBt(btTri)&&obonFlag&&btTrF){
nodoame 19:cd036cc40a2d 257 kRed=shotToDenziben(0x02,15,1,airAddr);
nodoame 19:cd036cc40a2d 258 wait(0.2f);
nodoame 19:cd036cc40a2d 259 kRed=shotToDenziben(0x02,0,1,airAddr);
nodoame 19:cd036cc40a2d 260 kRed=0;
nodoame 19:cd036cc40a2d 261 }
nodoame 19:cd036cc40a2d 262 else if(!getBt(btTri)){
nodoame 19:cd036cc40a2d 263 btTrF=true;
nodoame 4:bae4e2d103d4 264 kRed=0;
nodoame 3:c290afbacd1c 265 }
nodoame 3:c290afbacd1c 266
nodoame 19:cd036cc40a2d 267 //バルブ開放
nodoame 19:cd036cc40a2d 268 if(getPress(presLeft)&&pLeftF){
nodoame 19:cd036cc40a2d 269 pLeftF=false;
nodoame 19:cd036cc40a2d 270 kRed=shotToDenziben(7,1,1,valveAddr);
nodoame 19:cd036cc40a2d 271 wait(0.2);
nodoame 19:cd036cc40a2d 272 }
nodoame 19:cd036cc40a2d 273 else if(!getPress(presLeft)){
nodoame 4:bae4e2d103d4 274 kRed=0;
nodoame 19:cd036cc40a2d 275 pLeftF=true;
nodoame 3:c290afbacd1c 276 }
nodoame 19:cd036cc40a2d 277
nodoame 19:cd036cc40a2d 278 //バルブ開放
nodoame 19:cd036cc40a2d 279 if(getPress(presRight)&&pRightF){
nodoame 19:cd036cc40a2d 280 pRightF=false;
nodoame 19:cd036cc40a2d 281 kRed=shotToDenziben(8,1,1,valveAddr);
nodoame 19:cd036cc40a2d 282 wait(0.2);
nodoame 19:cd036cc40a2d 283 }
nodoame 19:cd036cc40a2d 284 else if(!getPress(presRight)){
nodoame 19:cd036cc40a2d 285 kRed=0;
nodoame 19:cd036cc40a2d 286 pRightF=true;
nodoame 19:cd036cc40a2d 287 }
nodoame 19:cd036cc40a2d 288
nodoame 19:cd036cc40a2d 289 //バルブ開放
nodoame 19:cd036cc40a2d 290 if(getBt(btCro)&&btCrF){
nodoame 19:cd036cc40a2d 291 btCrF=false;
nodoame 19:cd036cc40a2d 292 kRed=shotToDenziben(9,1,1,valveAddr);
nodoame 19:cd036cc40a2d 293 wait(0.2);
nodoame 19:cd036cc40a2d 294 }
nodoame 19:cd036cc40a2d 295 else if(!getBt(btCro))
nodoame 19:cd036cc40a2d 296 {
nodoame 19:cd036cc40a2d 297 kRed=0;
nodoame 19:cd036cc40a2d 298 btCrF=true;
nodoame 19:cd036cc40a2d 299 }
nodoame 19:cd036cc40a2d 300
nodoame 19:cd036cc40a2d 301
nodoame 5:428b7ac86810 302
nodoame 5:428b7ac86810 303 //レーザーポインタ
nodoame 3:c290afbacd1c 304 if(getBt(btSqa)&&sqf){
nodoame 3:c290afbacd1c 305 sqf=0;
nodoame 3:c290afbacd1c 306 l=!l;
nodoame 4:bae4e2d103d4 307 kBlue=!kBlue;
nodoame 0:dc587be179e9 308 }
nodoame 3:c290afbacd1c 309 else if(!getBt(btSqa))
nodoame 3:c290afbacd1c 310 sqf=1;
nodoame 3:c290afbacd1c 311
nodoame 5:428b7ac86810 312 //タッチセンサ
nodoame 12:6445a3c3d34c 313 if(((int)DATA[1]&12)==12){
nodoame 19:cd036cc40a2d 314 bValue=0.0f;gValue=0.8f;rValue=0;
nodoame 5:428b7ac86810 315 }
nodoame 18:e3591b0b9e40 316 else{
nodoame 18:e3591b0b9e40 317 bValue=0.8f;gValue=0;rValue=0;
nodoame 18:e3591b0b9e40 318 }
nodoame 19:cd036cc40a2d 319
nodoame 19:cd036cc40a2d 320 //お盆回し
nodoame 19:cd036cc40a2d 321 if(getPress(presUp)&&pUpF){
nodoame 19:cd036cc40a2d 322 kRed=shotToDenziben(0x02,7,1,airAddr);
nodoame 19:cd036cc40a2d 323 wait(0.2);
nodoame 19:cd036cc40a2d 324 obonFlag=true;
nodoame 19:cd036cc40a2d 325 pUpF=false;
nodoame 19:cd036cc40a2d 326 rValue=0.8;gValue=0;bValue=0.8;
nodoame 19:cd036cc40a2d 327 }
nodoame 19:cd036cc40a2d 328 else if(!getPress(presUp)){
nodoame 19:cd036cc40a2d 329 pUpF=true;
nodoame 19:cd036cc40a2d 330 kRed=0;
nodoame 19:cd036cc40a2d 331 }
nodoame 19:cd036cc40a2d 332 if(getPress(presDown)&&pDownF&&obonFlag){
nodoame 19:cd036cc40a2d 333 kRed=shotToDenziben(0x02,0,1,airAddr);
nodoame 19:cd036cc40a2d 334 wait(0.2);
nodoame 19:cd036cc40a2d 335 bValue=0.8f;gValue=0;rValue=0;
nodoame 19:cd036cc40a2d 336 pDownF=false;
nodoame 19:cd036cc40a2d 337 obonFlag=false;
nodoame 19:cd036cc40a2d 338 }
nodoame 19:cd036cc40a2d 339 else if(!getPress(presDown)){
nodoame 19:cd036cc40a2d 340 pDownF=true;
nodoame 19:cd036cc40a2d 341 kRed=0;
nodoame 19:cd036cc40a2d 342 }
nodoame 18:e3591b0b9e40 343
nodoame 4:bae4e2d103d4 344 deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
nodoame 19:cd036cc40a2d 345 val=valueForMechanum(deg,devideNum,getBt(trigL),getBt(trigR),getBt(btSph));
nodoame 11:1ca2498815c9 346
nodoame 11:1ca2498815c9 347 //一定時間通信してないと停止
nodoame 11:1ca2498815c9 348 if(stopCounter>stopCount)
nodoame 11:1ca2498815c9 349 val=64;
nodoame 11:1ca2498815c9 350 else
nodoame 11:1ca2498815c9 351 stopCounter++;
nodoame 0:dc587be179e9 352 Mechanum.putc(val);
nodoame 12:6445a3c3d34c 353 setFCLED(rValue,gValue,bValue);
nodoame 0:dc587be179e9 354 wait(0.03f);
nodoame 11:1ca2498815c9 355 do{
nodoame 11:1ca2498815c9 356 if(!debugFlag)
nodoame 11:1ca2498815c9 357 break;
nodoame 18:e3591b0b9e40 358 //pc.printf("val:%d\r\n",val);
nodoame 11:1ca2498815c9 359 //printf("%lf:%d:%d\r\n",deg,(int)DATA[3],(int)DATA[2]);
nodoame 11:1ca2498815c9 360 //printf("%d\r\n",(int)DATA[0]);
nodoame 11:1ca2498815c9 361 //pc.printf("DATA[1]%d\r\n",(int)(DATA[1]&0x02));
nodoame 11:1ca2498815c9 362 //printf("deg:%lfX1:%d:bt%ddeg:%lfval:%d\r\n",deg,(int)DATA[1],getBt(btSqa),deg,val);
nodoame 11:1ca2498815c9 363 }while(false);
nodoame 0:dc587be179e9 364 }
nodoame 0:dc587be179e9 365 }