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

Dependencies:   mbed

Fork of Nucleo_PS3_Jikken_2 by 2015 robotic contest arakawa A

Committer:
nodoame
Date:
Mon Jul 20 08:57:05 2015 +0000
Revision:
0:dc587be179e9
Child:
1:a3bfd422ea9e
20150720;

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