記念祭のドローンのプログラム
Dependencies: mbed MPU6050 nRF24L01P FastPWM
main.cpp@0:2067d58d510b, 2018-01-06 (annotated)
- Committer:
- ritarosakai
- Date:
- Sat Jan 06 03:17:13 2018 +0000
- Revision:
- 0:2067d58d510b
- Child:
- 1:930f9e34f513
First version;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ritarosakai | 0:2067d58d510b | 1 | #include "mbed.h" |
ritarosakai | 0:2067d58d510b | 2 | #include "MPU6050.h" |
ritarosakai | 0:2067d58d510b | 3 | #include "nRF24L01P.h" |
ritarosakai | 0:2067d58d510b | 4 | #include "FastPWM.h" |
ritarosakai | 0:2067d58d510b | 5 | |
ritarosakai | 0:2067d58d510b | 6 | nRF24L01P nrf(A6,A5,A4,D6,D3,A1); |
ritarosakai | 0:2067d58d510b | 7 | MPU6050 mpu(D4,D5); |
ritarosakai | 0:2067d58d510b | 8 | DigitalOut cpu(D2);/*CPU負荷表示LED |
ritarosakai | 0:2067d58d510b | 9 | 点灯...ループ内エラー |
ritarosakai | 0:2067d58d510b | 10 | 薄く点灯...ループ回転中 |
ritarosakai | 0:2067d58d510b | 11 | 0.2秒に0.1秒点灯...ジャイロセンサーのオフセット計測中 |
ritarosakai | 0:2067d58d510b | 12 | 0.6秒に0.1秒点灯...無線断絶 |
ritarosakai | 0:2067d58d510b | 13 | 1秒に0.5秒点灯...加速度センサー認識エラー |
ritarosakai | 0:2067d58d510b | 14 | 2.1秒に0.1秒点灯...バッテリー低下 |
ritarosakai | 0:2067d58d510b | 15 | 消灯...電源未接続または過度な傾きによる制御停止 |
ritarosakai | 0:2067d58d510b | 16 | */ |
ritarosakai | 0:2067d58d510b | 17 | AnalogIn batin(A3);//バッテリー電圧(1/3.4倍) |
ritarosakai | 0:2067d58d510b | 18 | Serial pc(USBTX,USBRX); |
ritarosakai | 0:2067d58d510b | 19 | FastPWM pwm1(D9); |
ritarosakai | 0:2067d58d510b | 20 | FastPWM pwm2(D10); |
ritarosakai | 0:2067d58d510b | 21 | FastPWM pwm3(D11); |
ritarosakai | 0:2067d58d510b | 22 | FastPWM pwm4(D12); |
ritarosakai | 0:2067d58d510b | 23 | |
ritarosakai | 0:2067d58d510b | 24 | char rxdata[4],txdata[4]; |
ritarosakai | 0:2067d58d510b | 25 | Ticker sampler; |
ritarosakai | 0:2067d58d510b | 26 | float axin,ayin,azin,gxin,gyin,gzin; |
ritarosakai | 0:2067d58d510b | 27 | float ax,ay,az,gx,gy,gz,kx,ky,kz,gx_offset,gy_offset,gz_offset,out,gx_old,gy_old; |
ritarosakai | 0:2067d58d510b | 28 | float bat,px,py,ix,iy,dx,dy,ddx,ddy,dx_old,dy_old,kxin,kyin,axin_old,ayin_old,kx_old,ky_old; |
ritarosakai | 0:2067d58d510b | 29 | float out1,out2,out3,out4,outx,outy,yo,yop,yoi; |
ritarosakai | 0:2067d58d510b | 30 | float pxgain=0.01,pygain=0.01;//PID制御のパラメーター |
ritarosakai | 0:2067d58d510b | 31 | float ixgain=0.01,iygain=0.01; |
ritarosakai | 0:2067d58d510b | 32 | float dxgain=0.003,dygain=0.003; |
ritarosakai | 0:2067d58d510b | 33 | float ddxgain=0,ddygain=0;//PIDD制御用 |
ritarosakai | 0:2067d58d510b | 34 | float yopgain=0.02;//ヨー方向PI制御パラメーター |
ritarosakai | 0:2067d58d510b | 35 | float yoigain=0.02; |
ritarosakai | 0:2067d58d510b | 36 | float z=-0.1;//上昇値の初期値 |
ritarosakai | 0:2067d58d510b | 37 | float samp=0.005;//制御周波数(s) |
ritarosakai | 0:2067d58d510b | 38 | float maxout=0.99f;//モーターPWM最大 |
ritarosakai | 0:2067d58d510b | 39 | short i,res; |
ritarosakai | 0:2067d58d510b | 40 | int times,nosignal; |
ritarosakai | 0:2067d58d510b | 41 | bool enable=1; |
ritarosakai | 0:2067d58d510b | 42 | bool timelimit=1;//タイムカウント有効 |
ritarosakai | 0:2067d58d510b | 43 | float rxkx,rxky; |
ritarosakai | 0:2067d58d510b | 44 | |
ritarosakai | 0:2067d58d510b | 45 | void sample() |
ritarosakai | 0:2067d58d510b | 46 | { |
ritarosakai | 0:2067d58d510b | 47 | cpu=1;//LED点灯 |
ritarosakai | 0:2067d58d510b | 48 | nosignal++; |
ritarosakai | 0:2067d58d510b | 49 | mpu.read(&gxin,&gyin,&gzin,&axin,&ayin,&azin);//6軸センサー読み取り |
ritarosakai | 0:2067d58d510b | 50 | bat=(batin.read()*12.65f + 0.2f)*0.1f+bat*0.9f;//バッテリー電圧読み取り |
ritarosakai | 0:2067d58d510b | 51 | if(nrf.readable()) { |
ritarosakai | 0:2067d58d510b | 52 | nrf.read( NRF24L01P_PIPE_P0,rxdata,sizeof(rxdata)); |
ritarosakai | 0:2067d58d510b | 53 | //pc.printf("%2d,%2d,%2d,%2d\r\n",rxdata[0],rxdata[1],rxdata[2],rxdata[3]); |
ritarosakai | 0:2067d58d510b | 54 | } |
ritarosakai | 0:2067d58d510b | 55 | if(rxdata[3]==1) { |
ritarosakai | 0:2067d58d510b | 56 | //times=500; |
ritarosakai | 0:2067d58d510b | 57 | nosignal=0;//nosignalをリセット |
ritarosakai | 0:2067d58d510b | 58 | rxdata[3]=0; |
ritarosakai | 0:2067d58d510b | 59 | } |
ritarosakai | 0:2067d58d510b | 60 | if(rxdata[0]==0) { |
ritarosakai | 0:2067d58d510b | 61 | rxdata[0]=128;//デフォルト128 |
ritarosakai | 0:2067d58d510b | 62 | } |
ritarosakai | 0:2067d58d510b | 63 | if(rxdata[1]==0) { |
ritarosakai | 0:2067d58d510b | 64 | rxdata[1]=128;//デフォルト128 |
ritarosakai | 0:2067d58d510b | 65 | } |
ritarosakai | 0:2067d58d510b | 66 | z=rxdata[2]*0.00485f + 0.3f;//上昇のモーターオフセットの計算 |
ritarosakai | 0:2067d58d510b | 67 | if(z<0.4f){//可変抵抗が一定値以下のときはモーター出力0,角度の積分値リセット |
ritarosakai | 0:2067d58d510b | 68 | z=-0.1f; |
ritarosakai | 0:2067d58d510b | 69 | times=500; |
ritarosakai | 0:2067d58d510b | 70 | ix=0; |
ritarosakai | 0:2067d58d510b | 71 | iy=0; |
ritarosakai | 0:2067d58d510b | 72 | yoi=0; |
ritarosakai | 0:2067d58d510b | 73 | kz=0; |
ritarosakai | 0:2067d58d510b | 74 | kxin=0; |
ritarosakai | 0:2067d58d510b | 75 | kyin=0; |
ritarosakai | 0:2067d58d510b | 76 | } |
ritarosakai | 0:2067d58d510b | 77 | /*axin=axin*0.1f + axin_old*0.9f; |
ritarosakai | 0:2067d58d510b | 78 | ayin=ayin*0.1f + ayin_old*0.9f; |
ritarosakai | 0:2067d58d510b | 79 | if(axin>1) { |
ritarosakai | 0:2067d58d510b | 80 | axin=1; |
ritarosakai | 0:2067d58d510b | 81 | } else if(axin<-1) { |
ritarosakai | 0:2067d58d510b | 82 | axin=-1; |
ritarosakai | 0:2067d58d510b | 83 | } |
ritarosakai | 0:2067d58d510b | 84 | if(ayin>1) { |
ritarosakai | 0:2067d58d510b | 85 | ayin=1; |
ritarosakai | 0:2067d58d510b | 86 | } else if(ayin<-1) { |
ritarosakai | 0:2067d58d510b | 87 | ayin=-1; |
ritarosakai | 0:2067d58d510b | 88 | }*/ |
ritarosakai | 0:2067d58d510b | 89 | gx=-gyin+gy_offset; |
ritarosakai | 0:2067d58d510b | 90 | gy=gxin-gx_offset; |
ritarosakai | 0:2067d58d510b | 91 | gz=gzin-gz_offset; |
ritarosakai | 0:2067d58d510b | 92 | //ax=asin(axin)*57.3f; |
ritarosakai | 0:2067d58d510b | 93 | //ay=asin(ayin)*57.3f; |
ritarosakai | 0:2067d58d510b | 94 | kxin=(kxin+gx*samp)*0.999f;//ジャイロセンサーの値から角度計算 |
ritarosakai | 0:2067d58d510b | 95 | kyin=(kyin+gy*samp)*0.999f; |
ritarosakai | 0:2067d58d510b | 96 | kz=kz+(gz*samp);//ヨー軸の角度計算 |
ritarosakai | 0:2067d58d510b | 97 | rxkx=(128-rxdata[0])*0.4f;//x軸の角度の指令値 |
ritarosakai | 0:2067d58d510b | 98 | rxky=(128-rxdata[1])*0.4f;//y軸の角度の指令値 |
ritarosakai | 0:2067d58d510b | 99 | kx=kxin+rxkx-gx_offset;//x軸の角度の指令値との差分を計算 |
ritarosakai | 0:2067d58d510b | 100 | ky=kyin+rxky-gy_offset;//y軸の角度の指令値との差分を計算 |
ritarosakai | 0:2067d58d510b | 101 | px=kx;//PIDのp |
ritarosakai | 0:2067d58d510b | 102 | py=ky; |
ritarosakai | 0:2067d58d510b | 103 | ix=ix+(px*samp);//PIDのi |
ritarosakai | 0:2067d58d510b | 104 | iy=iy+(py*samp); |
ritarosakai | 0:2067d58d510b | 105 | dx=(kx-kx_old)/samp;//PIDのd |
ritarosakai | 0:2067d58d510b | 106 | dy=(ky-ky_old)/samp; |
ritarosakai | 0:2067d58d510b | 107 | yop=kz;//ヨー軸のPIのp |
ritarosakai | 0:2067d58d510b | 108 | yoi=yoigain+kz*samp;//ヨー軸のPIのi |
ritarosakai | 0:2067d58d510b | 109 | yo=yop*yopgain + yoigain*yoigain;//ヨー軸のPI計算 |
ritarosakai | 0:2067d58d510b | 110 | ddx=(dx-dx_old)/samp;//PIDD制御用のDD |
ritarosakai | 0:2067d58d510b | 111 | ddy=(dy-dy_old)/samp; |
ritarosakai | 0:2067d58d510b | 112 | outx=(px*pxgain) + (ix*ixgain) + (dx*dxgain) + (ddx*ddxgain);//PIDでx軸計算 |
ritarosakai | 0:2067d58d510b | 113 | outy=(py*pygain) + (iy*iygain) + (dy*dygain) + (ddy*ddygain);//PIDでy軸計算 |
ritarosakai | 0:2067d58d510b | 114 | out1=outx + z+yo;//出力値計算 |
ritarosakai | 0:2067d58d510b | 115 | out2=-outx + z+yo; |
ritarosakai | 0:2067d58d510b | 116 | out3=outy + z-yo; |
ritarosakai | 0:2067d58d510b | 117 | out4=-outy + z-yo; |
ritarosakai | 0:2067d58d510b | 118 | if(out1>maxout) {//モーターのPWM値の制限 |
ritarosakai | 0:2067d58d510b | 119 | out1=maxout; |
ritarosakai | 0:2067d58d510b | 120 | } else if(out1<0) { |
ritarosakai | 0:2067d58d510b | 121 | out1=0; |
ritarosakai | 0:2067d58d510b | 122 | } |
ritarosakai | 0:2067d58d510b | 123 | if(out2>maxout) { |
ritarosakai | 0:2067d58d510b | 124 | out2=maxout; |
ritarosakai | 0:2067d58d510b | 125 | } else if(out2<0) { |
ritarosakai | 0:2067d58d510b | 126 | out2=0; |
ritarosakai | 0:2067d58d510b | 127 | } |
ritarosakai | 0:2067d58d510b | 128 | if(out3>maxout) { |
ritarosakai | 0:2067d58d510b | 129 | out3=maxout; |
ritarosakai | 0:2067d58d510b | 130 | } else if(out3<0) { |
ritarosakai | 0:2067d58d510b | 131 | out3=0; |
ritarosakai | 0:2067d58d510b | 132 | } |
ritarosakai | 0:2067d58d510b | 133 | if(out4>maxout) { |
ritarosakai | 0:2067d58d510b | 134 | out4=maxout; |
ritarosakai | 0:2067d58d510b | 135 | } else if(out4<0) { |
ritarosakai | 0:2067d58d510b | 136 | out4=0; |
ritarosakai | 0:2067d58d510b | 137 | } |
ritarosakai | 0:2067d58d510b | 138 | if(times==499){//飛行前に積分値リセット |
ritarosakai | 0:2067d58d510b | 139 | ix=0; |
ritarosakai | 0:2067d58d510b | 140 | iy=0; |
ritarosakai | 0:2067d58d510b | 141 | yoi=0; |
ritarosakai | 0:2067d58d510b | 142 | kz=0; |
ritarosakai | 0:2067d58d510b | 143 | } |
ritarosakai | 0:2067d58d510b | 144 | if(bat<6.2f){//バッテリー低下で飛行を中止 |
ritarosakai | 0:2067d58d510b | 145 | enable=0; |
ritarosakai | 0:2067d58d510b | 146 | } |
ritarosakai | 0:2067d58d510b | 147 | if(nosignal>20){//無線通信切断 |
ritarosakai | 0:2067d58d510b | 148 | enable=0; |
ritarosakai | 0:2067d58d510b | 149 | } |
ritarosakai | 0:2067d58d510b | 150 | if(z<0){//z<0の時モーターを停止 |
ritarosakai | 0:2067d58d510b | 151 | out1=0; |
ritarosakai | 0:2067d58d510b | 152 | out2=0; |
ritarosakai | 0:2067d58d510b | 153 | out3=0; |
ritarosakai | 0:2067d58d510b | 154 | out4=0; |
ritarosakai | 0:2067d58d510b | 155 | } |
ritarosakai | 0:2067d58d510b | 156 | if(enable==1&×>499) {//モーターを回転 |
ritarosakai | 0:2067d58d510b | 157 | pwm1=out1; |
ritarosakai | 0:2067d58d510b | 158 | pwm2=out2; |
ritarosakai | 0:2067d58d510b | 159 | pwm3=out3; |
ritarosakai | 0:2067d58d510b | 160 | pwm4=out4; |
ritarosakai | 0:2067d58d510b | 161 | }else{ |
ritarosakai | 0:2067d58d510b | 162 | pwm1=0; |
ritarosakai | 0:2067d58d510b | 163 | pwm2=0; |
ritarosakai | 0:2067d58d510b | 164 | pwm3=0; |
ritarosakai | 0:2067d58d510b | 165 | pwm4=0; |
ritarosakai | 0:2067d58d510b | 166 | } |
ritarosakai | 0:2067d58d510b | 167 | if(enable==0){ |
ritarosakai | 0:2067d58d510b | 168 | sampler.detach(); |
ritarosakai | 0:2067d58d510b | 169 | if(nosignal>20){ |
ritarosakai | 0:2067d58d510b | 170 | while(1){//0.6秒に0.1秒点灯...無線断絶 |
ritarosakai | 0:2067d58d510b | 171 | cpu=1; |
ritarosakai | 0:2067d58d510b | 172 | wait(0.1); |
ritarosakai | 0:2067d58d510b | 173 | cpu=0; |
ritarosakai | 0:2067d58d510b | 174 | wait(0.5); |
ritarosakai | 0:2067d58d510b | 175 | } |
ritarosakai | 0:2067d58d510b | 176 | }else if(bat<6.4f&&bat>3.1f){ |
ritarosakai | 0:2067d58d510b | 177 | while(1){//2.1秒に0.1秒点灯...バッテリー低下 |
ritarosakai | 0:2067d58d510b | 178 | cpu=1; |
ritarosakai | 0:2067d58d510b | 179 | wait(0.1); |
ritarosakai | 0:2067d58d510b | 180 | cpu=0; |
ritarosakai | 0:2067d58d510b | 181 | wait(2); |
ritarosakai | 0:2067d58d510b | 182 | } |
ritarosakai | 0:2067d58d510b | 183 | }else if(bat<3.1f){ |
ritarosakai | 0:2067d58d510b | 184 | sampler.attach(&sample,samp);//バッテリー未接続...ループ再開 |
ritarosakai | 0:2067d58d510b | 185 | } |
ritarosakai | 0:2067d58d510b | 186 | } |
ritarosakai | 0:2067d58d510b | 187 | if(abs(kx)>30){//30度以上傾いたら制御停止 |
ritarosakai | 0:2067d58d510b | 188 | enable=0; |
ritarosakai | 0:2067d58d510b | 189 | } |
ritarosakai | 0:2067d58d510b | 190 | if(abs(ky)>30){ |
ritarosakai | 0:2067d58d510b | 191 | enable=0; |
ritarosakai | 0:2067d58d510b | 192 | } |
ritarosakai | 0:2067d58d510b | 193 | if(abs(kz)>90){//z軸方向に90度以上回ったら制御停止 |
ritarosakai | 0:2067d58d510b | 194 | enable=0; |
ritarosakai | 0:2067d58d510b | 195 | } |
ritarosakai | 0:2067d58d510b | 196 | if(timelimit==1) { |
ritarosakai | 0:2067d58d510b | 197 | times++;//タイマーカウント |
ritarosakai | 0:2067d58d510b | 198 | } |
ritarosakai | 0:2067d58d510b | 199 | //axin_old=axin; |
ritarosakai | 0:2067d58d510b | 200 | //ayin_old=ayin; |
ritarosakai | 0:2067d58d510b | 201 | gx_old=gx; |
ritarosakai | 0:2067d58d510b | 202 | gy_old=gy; |
ritarosakai | 0:2067d58d510b | 203 | dx_old=dx; |
ritarosakai | 0:2067d58d510b | 204 | dy_old=dy; |
ritarosakai | 0:2067d58d510b | 205 | kx_old=kx; |
ritarosakai | 0:2067d58d510b | 206 | ky_old=ky; |
ritarosakai | 0:2067d58d510b | 207 | pc.printf("%.2f,%.2f\r\n",kx,ky); |
ritarosakai | 0:2067d58d510b | 208 | txdata[0]=outx*100+128;//制御データーを送信 |
ritarosakai | 0:2067d58d510b | 209 | txdata[1]=outy*100+128; |
ritarosakai | 0:2067d58d510b | 210 | txdata[2]=bat*20; |
ritarosakai | 0:2067d58d510b | 211 | txdata[3]=1; |
ritarosakai | 0:2067d58d510b | 212 | nrf.write( NRF24L01P_PIPE_P0, txdata, 4 ); |
ritarosakai | 0:2067d58d510b | 213 | cpu=0; |
ritarosakai | 0:2067d58d510b | 214 | } |
ritarosakai | 0:2067d58d510b | 215 | |
ritarosakai | 0:2067d58d510b | 216 | int main() |
ritarosakai | 0:2067d58d510b | 217 | { |
ritarosakai | 0:2067d58d510b | 218 | cpu=1; |
ritarosakai | 0:2067d58d510b | 219 | pc.baud(230400); |
ritarosakai | 0:2067d58d510b | 220 | pwm1.period_us(25);//PWM周波数...40kHz |
ritarosakai | 0:2067d58d510b | 221 | pwm2.period_us(25); |
ritarosakai | 0:2067d58d510b | 222 | pwm3.period_us(25); |
ritarosakai | 0:2067d58d510b | 223 | pwm4.period_us(25); |
ritarosakai | 0:2067d58d510b | 224 | if(mpu.getID()==0x68) { |
ritarosakai | 0:2067d58d510b | 225 | pc.printf("MPU6050 OK Address=0x%x\r\n",mpu.getID()); |
ritarosakai | 0:2067d58d510b | 226 | } else { |
ritarosakai | 0:2067d58d510b | 227 | pc.printf("MPU6050 error ID=0x%x\r\n",mpu.getID()); |
ritarosakai | 0:2067d58d510b | 228 | while(1) { |
ritarosakai | 0:2067d58d510b | 229 | cpu=1; |
ritarosakai | 0:2067d58d510b | 230 | wait(0.5);//1秒に0.5秒点灯...加速度センサーエラー |
ritarosakai | 0:2067d58d510b | 231 | cpu=0; |
ritarosakai | 0:2067d58d510b | 232 | wait(0.5); |
ritarosakai | 0:2067d58d510b | 233 | } |
ritarosakai | 0:2067d58d510b | 234 | } |
ritarosakai | 0:2067d58d510b | 235 | wait(0.1); |
ritarosakai | 0:2067d58d510b | 236 | nrf.powerUp(); |
ritarosakai | 0:2067d58d510b | 237 | nrf.setRxAddress(0xe7e7e7e5a4); |
ritarosakai | 0:2067d58d510b | 238 | nrf.setTxAddress(0xe7e7e7e7e6); |
ritarosakai | 0:2067d58d510b | 239 | nrf.setAirDataRate(250); |
ritarosakai | 0:2067d58d510b | 240 | pc.printf( "%dMHz %ddbm %dKbps ", nrf.getRfFrequency(),nrf.getRfOutputPower(),nrf.getAirDataRate()); |
ritarosakai | 0:2067d58d510b | 241 | pc.printf("Tx:0x%010llX ",nrf.getTxAddress()); |
ritarosakai | 0:2067d58d510b | 242 | pc.printf("Rx:0x%010llX \r\n",nrf.getRxAddress()); |
ritarosakai | 0:2067d58d510b | 243 | nrf.setTransferSize(4); |
ritarosakai | 0:2067d58d510b | 244 | nrf.setReceiveMode(); |
ritarosakai | 0:2067d58d510b | 245 | nrf.enable(); |
ritarosakai | 0:2067d58d510b | 246 | bat=batin.read(); |
ritarosakai | 0:2067d58d510b | 247 | bat=bat*12.65f + 0.2f; |
ritarosakai | 0:2067d58d510b | 248 | txdata[0]=kx+128; |
ritarosakai | 0:2067d58d510b | 249 | txdata[1]=ky+128; |
ritarosakai | 0:2067d58d510b | 250 | txdata[2]=bat*20; |
ritarosakai | 0:2067d58d510b | 251 | txdata[3]=1; |
ritarosakai | 0:2067d58d510b | 252 | nrf.write( NRF24L01P_PIPE_P0, txdata, 4 );//初期値送信 |
ritarosakai | 0:2067d58d510b | 253 | cpu=0; |
ritarosakai | 0:2067d58d510b | 254 | wait(0.2); |
ritarosakai | 0:2067d58d510b | 255 | mpu.start(); |
ritarosakai | 0:2067d58d510b | 256 | bat=batin.read(); |
ritarosakai | 0:2067d58d510b | 257 | bat=bat*12.65f + 0.2f; |
ritarosakai | 0:2067d58d510b | 258 | pc.printf("Battery %4.2fV\r\n",bat); |
ritarosakai | 0:2067d58d510b | 259 | if(bat<6.4f) { |
ritarosakai | 0:2067d58d510b | 260 | enable=0; |
ritarosakai | 0:2067d58d510b | 261 | pc.printf("Battery is low!\r\n"); |
ritarosakai | 0:2067d58d510b | 262 | } |
ritarosakai | 0:2067d58d510b | 263 | txdata[0]=kx+128; |
ritarosakai | 0:2067d58d510b | 264 | txdata[1]=ky+128; |
ritarosakai | 0:2067d58d510b | 265 | txdata[2]=bat*20; |
ritarosakai | 0:2067d58d510b | 266 | txdata[3]=1; |
ritarosakai | 0:2067d58d510b | 267 | nrf.write( NRF24L01P_PIPE_P0, txdata, 4 ); |
ritarosakai | 0:2067d58d510b | 268 | wait(0.5); |
ritarosakai | 0:2067d58d510b | 269 | cpu=1; |
ritarosakai | 0:2067d58d510b | 270 | while(i<100) {//ジャイロセンサーのオフセット測定 |
ritarosakai | 0:2067d58d510b | 271 | mpu.read(&gxin,&gyin,&gzin,&axin,&ayin,&azin); |
ritarosakai | 0:2067d58d510b | 272 | gx_offset=gx_offset+gxin; |
ritarosakai | 0:2067d58d510b | 273 | gy_offset=gy_offset+gyin; |
ritarosakai | 0:2067d58d510b | 274 | gz_offset=gz_offset+gzin; |
ritarosakai | 0:2067d58d510b | 275 | times++; |
ritarosakai | 0:2067d58d510b | 276 | if(times==10) { |
ritarosakai | 0:2067d58d510b | 277 | cpu=!cpu; |
ritarosakai | 0:2067d58d510b | 278 | times=0; |
ritarosakai | 0:2067d58d510b | 279 | } |
ritarosakai | 0:2067d58d510b | 280 | i++; |
ritarosakai | 0:2067d58d510b | 281 | wait(0.01); |
ritarosakai | 0:2067d58d510b | 282 | } |
ritarosakai | 0:2067d58d510b | 283 | i=0; |
ritarosakai | 0:2067d58d510b | 284 | gx_offset=gx_offset/100;//ジャイロセンサーのオフセット計算 |
ritarosakai | 0:2067d58d510b | 285 | gy_offset=gy_offset/100; |
ritarosakai | 0:2067d58d510b | 286 | gz_offset=gz_offset/100; |
ritarosakai | 0:2067d58d510b | 287 | cpu=0; |
ritarosakai | 0:2067d58d510b | 288 | //gx_offset=-2.63; |
ritarosakai | 0:2067d58d510b | 289 | //gy_offset=-0.71; |
ritarosakai | 0:2067d58d510b | 290 | pc.printf("gyro offset %.2f %.2f %.2f\r\n",gx_offset,gy_offset,gz_offset); |
ritarosakai | 0:2067d58d510b | 291 | wait(2); |
ritarosakai | 0:2067d58d510b | 292 | sampler.attach_us(&sample,samp*1000000);//ループ開始 |
ritarosakai | 0:2067d58d510b | 293 | while(1) { |
ritarosakai | 0:2067d58d510b | 294 | wait(0.01); |
ritarosakai | 0:2067d58d510b | 295 | } |
ritarosakai | 0:2067d58d510b | 296 | } |