2015 robotic contest arakawa A / Mbed 2 deprecated Nucleo_PS3_Jikken3

マザー 20151008 アンカーをジョイスティックで解除

Dependencies:   mbed

Fork of Nucleo_PS3_Jikken_2 by 2015 robotic contest arakawa A

main.cpp@4:bae4e2d103d4, 2015-08-27 (annotated)

Committer:
nodoame
Date:
Thu Aug 27 05:03:13 2015 +0000
Revision:
4:bae4e2d103d4
Parent:
3:c290afbacd1c
Child:
5:428b7ac86810
.;

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