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@7:ff092d85496c, 2015-09-16 (annotated)

Committer:
nodoame
Date:
Wed Sep 16 06:54:47 2015 +0000
Revision:
7:ff092d85496c
Parent:
6:5171a8245b57
Child:
8:b2ead9ecd6cd
???????????

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