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@2:c5f038d7be1f, 2015-08-09 (annotated)

Committer:
nodoame
Date:
Sun Aug 09 06:19:05 2015 +0000
Revision:
2:c5f038d7be1f
Parent:
1:a3bfd422ea9e
Child:
3:c290afbacd1c
20150809 ??????

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