マザー 20151008 アンカーをジョイスティックで解除
Dependencies: mbed
Fork of Nucleo_PS3_Jikken_2 by
main.cpp@0:dc587be179e9, 2015-07-20 (annotated)
- Committer:
- nodoame
- Date:
- Mon Jul 20 08:57:05 2015 +0000
- Revision:
- 0:dc587be179e9
- Child:
- 1:a3bfd422ea9e
20150720;
Who changed what in which revision?
User | Revision | Line number | New 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 | } |