2015 robotic contest arakawa A / Mbed 2 deprecated Nucleo_PS3_Jikken_XBEE

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of Nucleo_PS3_Jikken_XBOX by 2015 robotic contest arakawa A

main.cpp.orig@29:9cfd082de61e, 2015-10-30 (annotated)

Committer:
nodoame
Date:
Fri Oct 30 08:38:18 2015 +0000
Revision:
29:9cfd082de61e
Parent:
22:ab619079fb8d 
201510030; XBOX??

Who changed what in which revision?

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