2015 robotic contest arakawa A / Mbed 2 deprecated Nucleo_PS3_Jikken_PS3

マザー 20151028 PS3

Dependencies:   mbed

Fork of Nucleo_PS3_Jikken3 by 2015 robotic contest arakawa A

main.cpp@5:428b7ac86810, 2015-09-14 (annotated)

Committer:
nodoame
Date:
Mon Sep 14 02:32:41 2015 +0000
Revision:
5:428b7ac86810
Parent:
4:bae4e2d103d4
Child:
6:5171a8245b57
???????

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