2015 robotic contest arakawa A / Mbed 2 deprecated Nucleo_PS3_Jikken_XBEE

マザー 20151028 XBOX-XBEE

Dependencies:   mbed

Fork of Nucleo_PS3_Jikken_XBOX by 2015 robotic contest arakawa A

main.cpp@20:373fa71ce05b, 2015-10-16 (annotated)

Committer:
nodoame
Date:
Fri Oct 16 09:32:02 2015 +0000
Revision:
20:373fa71ce05b
Parent:
19:cd036cc40a2d
Child:
22:ab619079fb8d
??????????

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