2015 robotic contest arakawa A / Mbed 2 deprecated Nucleo_PS3_Jikken_new

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_XBEE by 2015 robotic contest arakawa A

main.cpp@38:8a1db149ee61, 2016-05-10 (annotated)

Committer:
nodoame
Date:
Tue May 10 09:25:13 2016 +0000
Revision:
38:8a1db149ee61
Parent:
37:42a4b925ca9c 
20160510;

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