main.cpp

00001 #include "mbed.h"
00002 #include "MPU6050.h"
00003 #include "nRF24L01P.h"
00004 #include "FastPWM.h"
00005 
00006 nRF24L01P nrf(A6,A5,A4,D6,D3,A1);
00007 MPU6050 mpu(D4,D5);
00008 DigitalOut cpu(D2);/*CPU負荷表示LED
00009 点灯...ループ内エラー
00010 薄く点灯...ループ回転中
00011 0.2秒に0.1秒点灯...ジャイロセンサーのオフセット計測中
00012 0.6秒に0.1秒点灯...無線断絶
00013 1秒に0.5秒点灯...加速度センサー認識エラー
00014 2.1秒に0.1秒点灯...バッテリー低下
00015 消灯...電源未接続または過度な傾きによる制御停止
00016 */
00017 AnalogIn batin(A3);//バッテリー電圧(1/3.4倍)
00018 Serial pc(USBTX,USBRX);
00019 FastPWM pwm1(D9);
00020 FastPWM pwm2(D10);
00021 FastPWM pwm3(D11);
00022 FastPWM pwm4(D12);
00023 
00024 char rxdata[4],txdata[4];
00025 Ticker sampler;
00026 float axin,ayin,azin,gxin,gyin,gzin;
00027 float ax,ay,az,gx,gy,gz,kx,ky,kz,gx_offset,gy_offset,gz_offset,out,gx_old,gy_old;
00028 float bat,px,py,ix,iy,dx,dy,ddx,ddy,dx_old,dy_old,kxin,kyin,axin_old,ayin_old,kx_old,ky_old;
00029 float out1,out2,out3,out4,outx,outy,yo,yop,yoi;
00030 float pxgain=0.01,pygain=0.01;//PID制御のパラメーター
00031 float ixgain=0.01,iygain=0.01;
00032 float dxgain=0.003,dygain=0.003;
00033 float ddxgain=0,ddygain=0;//PIDD制御用
00034 float yopgain=0.02;//ヨー方向PI制御パラメーター
00035 float yoigain=0.02;
00036 float z=-0.1;//上昇値の初期値
00037 float samp=0.005;//制御周波数(s)
00038 float maxout=0.99f;//モーターPWM最大
00039 short i,res;
00040 int times,nosignal;
00041 bool enable=1;
00042 bool timelimit=1;//タイムカウント有効
00043 float rxkx,rxky;
00044 
00045 void sample()
00046 {
00047     cpu=1;//LED点灯
00048     nosignal++;
00049     mpu.read(&gxin,&gyin,&gzin,&axin,&ayin,&azin);//6軸センサー読み取り
00050     bat=(batin.read()*12.65f + 0.2f)*0.1f+bat*0.9f;//バッテリー電圧読み取り
00051     if(nrf.readable()) {
00052         nrf.read( NRF24L01P_PIPE_P0,rxdata,sizeof(rxdata));
00053         //pc.printf("%2d,%2d,%2d,%2d\r\n",rxdata[0],rxdata[1],rxdata[2],rxdata[3]);
00054     }
00055     if(rxdata[3]==1) {
00056         //times=500;
00057         nosignal=0;//nosignalをリセット
00058         rxdata[3]=0;
00059     }
00060     if(rxdata[0]==0) {
00061         rxdata[0]=128;//デフォルト128
00062     }
00063     if(rxdata[1]==0) {
00064         rxdata[1]=128;//デフォルト128
00065     }
00066     z=rxdata[2]*0.00485f + 0.3f;//上昇のモーターオフセットの計算
00067     if(z<0.4f) { //可変抵抗が一定値以下のときはモーター出力0,角度の積分値リセット
00068         z=-0.1f;
00069         times=500;
00070         ix=0;
00071         iy=0;
00072         yoi=0;
00073         kz=0;
00074         kxin=0;
00075         kyin=0;
00076     }
00077     /*axin=axin*0.1f + axin_old*0.9f;
00078     ayin=ayin*0.1f + ayin_old*0.9f;
00079     if(axin>1) {
00080     axin=1;
00081     } else if(axin<-1) {
00082     axin=-1;
00083     }
00084     if(ayin>1) {
00085     ayin=1;
00086     } else if(ayin<-1) {
00087     ayin=-1;
00088     }*/
00089     gx=-gyin+gy_offset;
00090     gy=gxin-gx_offset;
00091     gz=gzin-gz_offset;
00092     //ax=asin(axin)*57.3f;
00093     //ay=asin(ayin)*57.3f;
00094     kxin=(kxin+gx*samp)*0.999f;//ジャイロセンサーの値から角度計算
00095     kyin=(kyin+gy*samp)*0.999f;
00096     kz=kz+(gz*samp);//ヨー軸の角度計算
00097     rxkx=(128-rxdata[0])*0.4f;//x軸の角度の指令値
00098     rxky=(128-rxdata[1])*0.4f;//y軸の角度の指令値
00099     kx=kxin+rxkx-gx_offset;//x軸の角度の指令値との差分を計算
00100     ky=kyin+rxky-gy_offset;//y軸の角度の指令値との差分を計算
00101     px=kx;//PIDのp
00102     py=ky;
00103     ix=ix+(px*samp);//PIDのi
00104     iy=iy+(py*samp);
00105     dx=(kx-kx_old)/samp;//PIDのd
00106     dy=(ky-ky_old)/samp;
00107     yop=kz;//ヨー軸のPIのp
00108     yoi=yoigain+kz*samp;//ヨー軸のPIのi
00109     yo=yop*yopgain + yoigain*yoigain;//ヨー軸のPI計算
00110     ddx=(dx-dx_old)/samp;//PIDD制御用のDD
00111     ddy=(dy-dy_old)/samp;
00112     outx=(px*pxgain) + (ix*ixgain) + (dx*dxgain) + (ddx*ddxgain);//PIDでx軸計算
00113     outy=(py*pygain) + (iy*iygain) + (dy*dygain) + (ddy*ddygain);//PIDでy軸計算
00114     out1=outx + z+yo;//出力値計算
00115     out2=-outx + z+yo;
00116     out3=outy + z-yo;
00117     out4=-outy + z-yo;
00118     if(out1>maxout) {//モーターのPWM値の制限
00119         out1=maxout;
00120     } else if(out1<0) {
00121         out1=0;
00122     }
00123     if(out2>maxout) {
00124         out2=maxout;
00125     } else if(out2<0) {
00126         out2=0;
00127     }
00128     if(out3>maxout) {
00129         out3=maxout;
00130     } else if(out3<0) {
00131         out3=0;
00132     }
00133     if(out4>maxout) {
00134         out4=maxout;
00135     } else if(out4<0) {
00136         out4=0;
00137     }
00138     if(times==499) { //飛行前に積分値リセット
00139         ix=0;
00140         iy=0;
00141         yoi=0;
00142         kz=0;
00143     }
00144     if(bat<6.2f) { //バッテリー低下で飛行を中止
00145         enable=0;
00146     }
00147     if(nosignal>20) { //無線通信切断
00148         enable=0;
00149     }
00150     if(z<0) { //z<0の時モーターを停止
00151         out1=0;
00152         out2=0;
00153         out3=0;
00154         out4=0;
00155     }
00156     if(enable==1&&times>499) {//モーターを回転
00157         pwm1=out1;
00158         pwm2=out2;
00159         pwm3=out3;
00160         pwm4=out4;
00161     } else {
00162         pwm1=0;
00163         pwm2=0;
00164         pwm3=0;
00165         pwm4=0;
00166     }
00167     if(enable==0) {
00168         sampler.detach();
00169         if(nosignal>20) {
00170             while(1) { //0.6秒に0.1秒点灯...無線断絶
00171                 cpu=1;
00172                 wait(0.1);
00173                 cpu=0;
00174                 wait(0.5);
00175             }
00176         } else if(bat<6.4f&&bat>3.1f) {
00177             while(1) { //2.1秒に0.1秒点灯...バッテリー低下
00178                 cpu=1;
00179                 wait(0.1);
00180                 cpu=0;
00181                 wait(2);
00182             }
00183         } else if(bat<3.1f) {
00184             sampler.attach(&sample,samp);//バッテリー未接続...ループ再開
00185         }
00186     }
00187     if(abs(kx)>30) { //30度以上傾いたら制御停止
00188         enable=0;
00189     }
00190     if(abs(ky)>30) {
00191         enable=0;
00192     }
00193     if(abs(kz)>90) { //z軸方向に90度以上回ったら制御停止
00194         enable=0;
00195     }
00196     if(timelimit==1) {
00197         times++;//タイマーカウント
00198     }
00199     //axin_old=axin;
00200     //ayin_old=ayin;
00201     gx_old=gx;
00202     gy_old=gy;
00203     dx_old=dx;
00204     dy_old=dy;
00205     kx_old=kx;
00206     ky_old=ky;
00207     //pc.printf("%.2f,%.2f\r\n",kx,ky);
00208     txdata[0]=outx*100+128;//制御データーを送信
00209     txdata[1]=outy*100+128;
00210     txdata[2]=bat*20;
00211     txdata[3]=1;
00212     nrf.write( NRF24L01P_PIPE_P0, txdata, 4 );
00213     cpu=0;
00214 }
00215 
00216 int main()
00217 {
00218     cpu=1;
00219     pc.baud(230400);
00220     pwm1.period_us(25);//PWM周波数...40kHz
00221     pwm2.period_us(25);
00222     pwm3.period_us(25);
00223     pwm4.period_us(25);
00224     if(mpu.getID()==0x68) {
00225         pc.printf("MPU6050 OK Address=0x%x\r\n",mpu.getID());
00226     } else {
00227         pc.printf("MPU6050 error ID=0x%x\r\n",mpu.getID());
00228         while(1) {
00229             cpu=1;
00230             wait(0.5);//1秒に0.5秒点灯...加速度センサーエラー
00231             cpu=0;
00232             wait(0.5);
00233         }
00234     }
00235     wait(0.1);
00236     nrf.powerUp();
00237     nrf.setRxAddress(0xe7e7e7e5a4);
00238     nrf.setTxAddress(0xe7e7e7e7e6);
00239     nrf.setAirDataRate(250);
00240     pc.printf( "%dMHz %ddbm %dKbps ",  nrf.getRfFrequency(),nrf.getRfOutputPower(),nrf.getAirDataRate());
00241     pc.printf("Tx:0x%010llX ",nrf.getTxAddress());
00242     pc.printf("Rx:0x%010llX \r\n",nrf.getRxAddress());
00243     nrf.setTransferSize(4);
00244     nrf.setReceiveMode();
00245     nrf.enable();
00246     bat=batin.read();
00247     bat=bat*12.65f + 0.2f;
00248     txdata[0]=kx+128;
00249     txdata[1]=ky+128;
00250     txdata[2]=bat*20;
00251     txdata[3]=1;
00252     nrf.write( NRF24L01P_PIPE_P0, txdata, 4 );//初期値送信
00253     cpu=0;
00254     wait(0.2);
00255     mpu.start();
00256     bat=batin.read();
00257     bat=bat*12.65f + 0.2f;
00258     pc.printf("Battery %4.2fV\r\n",bat);
00259     if(bat<6.4f) {
00260         enable=0;
00261         pc.printf("Battery is low!\r\n");
00262     }
00263     txdata[0]=kx+128;
00264     txdata[1]=ky+128;
00265     txdata[2]=bat*20;
00266     txdata[3]=1;
00267     nrf.write( NRF24L01P_PIPE_P0, txdata, 4 );
00268     wait(0.5);
00269     cpu=1;
00270     while(i<100) {//ジャイロセンサーのオフセット測定
00271         mpu.read(&gxin,&gyin,&gzin,&axin,&ayin,&azin);
00272         gx_offset=gx_offset+gxin;
00273         gy_offset=gy_offset+gyin;
00274         gz_offset=gz_offset+gzin;
00275         times++;
00276         if(times==10) {
00277             cpu=!cpu;
00278             times=0;
00279         }
00280         i++;
00281         wait(0.01);
00282     }
00283     i=0;
00284     gx_offset=gx_offset/100;//ジャイロセンサーのオフセット計算
00285     gy_offset=gy_offset/100;
00286     gz_offset=gz_offset/100;
00287     cpu=0;
00288     //gx_offset=-2.63;
00289     //gy_offset=-0.71;
00290     pc.printf("gyro offset %.2f %.2f %.2f\r\n",gx_offset,gy_offset,gz_offset);
00291     wait(2);
00292     //sampler.attach_us(&sample,samp*1000000);//ループ開始
00293     while(1) {
00294         sample();
00295         wait_us(3500);
00296     }
00297 }