File content as of revision 1:930f9e34f513:

#include "mbed.h"
#include "MPU6050.h"
#include "nRF24L01P.h"
#include "FastPWM.h"

nRF24L01P nrf(A6,A5,A4,D6,D3,A1);
MPU6050 mpu(D4,D5);
DigitalOut cpu(D2);/*CPU負荷表示LED
点灯...ループ内エラー
薄く点灯...ループ回転中
0.2秒に0.1秒点灯...ジャイロセンサーのオフセット計測中
0.6秒に0.1秒点灯...無線断絶
1秒に0.5秒点灯...加速度センサー認識エラー
2.1秒に0.1秒点灯...バッテリー低下
消灯...電源未接続または過度な傾きによる制御停止
*/
AnalogIn batin(A3);//バッテリー電圧(1/3.4倍)
Serial pc(USBTX,USBRX);
FastPWM pwm1(D9);
FastPWM pwm2(D10);
FastPWM pwm3(D11);
FastPWM pwm4(D12);

char rxdata[4],txdata[4];
Ticker sampler;
float axin,ayin,azin,gxin,gyin,gzin;
float ax,ay,az,gx,gy,gz,kx,ky,kz,gx_offset,gy_offset,gz_offset,out,gx_old,gy_old;
float bat,px,py,ix,iy,dx,dy,ddx,ddy,dx_old,dy_old,kxin,kyin,axin_old,ayin_old,kx_old,ky_old;
float out1,out2,out3,out4,outx,outy,yo,yop,yoi;
float pxgain=0.01,pygain=0.01;//PID制御のパラメーター
float ixgain=0.01,iygain=0.01;
float dxgain=0.003,dygain=0.003;
float ddxgain=0,ddygain=0;//PIDD制御用
float yopgain=0.02;//ヨー方向PI制御パラメーター
float yoigain=0.02;
float z=-0.1;//上昇値の初期値
float samp=0.005;//制御周波数(s)
float maxout=0.99f;//モーターPWM最大
short i,res;
int times,nosignal;
bool enable=1;
bool timelimit=1;//タイムカウント有効
float rxkx,rxky;

void sample()
{
    cpu=1;//LED点灯
    nosignal++;
    mpu.read(&gxin,&gyin,&gzin,&axin,&ayin,&azin);//6軸センサー読み取り
    bat=(batin.read()*12.65f + 0.2f)*0.1f+bat*0.9f;//バッテリー電圧読み取り
    if(nrf.readable()) {
        nrf.read( NRF24L01P_PIPE_P0,rxdata,sizeof(rxdata));
        //pc.printf("%2d,%2d,%2d,%2d\r\n",rxdata[0],rxdata[1],rxdata[2],rxdata[3]);
    }
    if(rxdata[3]==1) {
        //times=500;
        nosignal=0;//nosignalをリセット
        rxdata[3]=0;
    }
    if(rxdata[0]==0) {
        rxdata[0]=128;//デフォルト128
    }
    if(rxdata[1]==0) {
        rxdata[1]=128;//デフォルト128
    }
    z=rxdata[2]*0.00485f + 0.3f;//上昇のモーターオフセットの計算
    if(z<0.4f) { //可変抵抗が一定値以下のときはモーター出力0,角度の積分値リセット
        z=-0.1f;
        times=500;
        ix=0;
        iy=0;
        yoi=0;
        kz=0;
        kxin=0;
        kyin=0;
    }
    /*axin=axin*0.1f + axin_old*0.9f;
    ayin=ayin*0.1f + ayin_old*0.9f;
    if(axin>1) {
    axin=1;
    } else if(axin<-1) {
    axin=-1;
    }
    if(ayin>1) {
    ayin=1;
    } else if(ayin<-1) {
    ayin=-1;
    }*/
    gx=-gyin+gy_offset;
    gy=gxin-gx_offset;
    gz=gzin-gz_offset;
    //ax=asin(axin)*57.3f;
    //ay=asin(ayin)*57.3f;
    kxin=(kxin+gx*samp)*0.999f;//ジャイロセンサーの値から角度計算
    kyin=(kyin+gy*samp)*0.999f;
    kz=kz+(gz*samp);//ヨー軸の角度計算
    rxkx=(128-rxdata[0])*0.4f;//x軸の角度の指令値
    rxky=(128-rxdata[1])*0.4f;//y軸の角度の指令値
    kx=kxin+rxkx-gx_offset;//x軸の角度の指令値との差分を計算
    ky=kyin+rxky-gy_offset;//y軸の角度の指令値との差分を計算
    px=kx;//PIDのp
    py=ky;
    ix=ix+(px*samp);//PIDのi
    iy=iy+(py*samp);
    dx=(kx-kx_old)/samp;//PIDのd
    dy=(ky-ky_old)/samp;
    yop=kz;//ヨー軸のPIのp
    yoi=yoigain+kz*samp;//ヨー軸のPIのi
    yo=yop*yopgain + yoigain*yoigain;//ヨー軸のPI計算
    ddx=(dx-dx_old)/samp;//PIDD制御用のDD
    ddy=(dy-dy_old)/samp;
    outx=(px*pxgain) + (ix*ixgain) + (dx*dxgain) + (ddx*ddxgain);//PIDでx軸計算
    outy=(py*pygain) + (iy*iygain) + (dy*dygain) + (ddy*ddygain);//PIDでy軸計算
    out1=outx + z+yo;//出力値計算
    out2=-outx + z+yo;
    out3=outy + z-yo;
    out4=-outy + z-yo;
    if(out1>maxout) {//モーターのPWM値の制限
        out1=maxout;
    } else if(out1<0) {
        out1=0;
    }
    if(out2>maxout) {
        out2=maxout;
    } else if(out2<0) {
        out2=0;
    }
    if(out3>maxout) {
        out3=maxout;
    } else if(out3<0) {
        out3=0;
    }
    if(out4>maxout) {
        out4=maxout;
    } else if(out4<0) {
        out4=0;
    }
    if(times==499) { //飛行前に積分値リセット
        ix=0;
        iy=0;
        yoi=0;
        kz=0;
    }
    if(bat<6.2f) { //バッテリー低下で飛行を中止
        enable=0;
    }
    if(nosignal>20) { //無線通信切断
        enable=0;
    }
    if(z<0) { //z<0の時モーターを停止
        out1=0;
        out2=0;
        out3=0;
        out4=0;
    }
    if(enable==1&&times>499) {//モーターを回転
        pwm1=out1;
        pwm2=out2;
        pwm3=out3;
        pwm4=out4;
    } else {
        pwm1=0;
        pwm2=0;
        pwm3=0;
        pwm4=0;
    }
    if(enable==0) {
        sampler.detach();
        if(nosignal>20) {
            while(1) { //0.6秒に0.1秒点灯...無線断絶
                cpu=1;
                wait(0.1);
                cpu=0;
                wait(0.5);
            }
        } else if(bat<6.4f&&bat>3.1f) {
            while(1) { //2.1秒に0.1秒点灯...バッテリー低下
                cpu=1;
                wait(0.1);
                cpu=0;
                wait(2);
            }
        } else if(bat<3.1f) {
            sampler.attach(&sample,samp);//バッテリー未接続...ループ再開
        }
    }
    if(abs(kx)>30) { //30度以上傾いたら制御停止
        enable=0;
    }
    if(abs(ky)>30) {
        enable=0;
    }
    if(abs(kz)>90) { //z軸方向に90度以上回ったら制御停止
        enable=0;
    }
    if(timelimit==1) {
        times++;//タイマーカウント
    }
    //axin_old=axin;
    //ayin_old=ayin;
    gx_old=gx;
    gy_old=gy;
    dx_old=dx;
    dy_old=dy;
    kx_old=kx;
    ky_old=ky;
    pc.printf("%.2f,%.2f\r\n",kx,ky);
    txdata[0]=outx*100+128;//制御データーを送信
    txdata[1]=outy*100+128;
    txdata[2]=bat*20;
    txdata[3]=1;
    nrf.write( NRF24L01P_PIPE_P0, txdata, 4 );
    cpu=0;
}

int main()
{
    cpu=1;
    pc.baud(230400);
    pwm1.period_us(25);//PWM周波数...40kHz
    pwm2.period_us(25);
    pwm3.period_us(25);
    pwm4.period_us(25);
    if(mpu.getID()==0x68) {
        pc.printf("MPU6050 OK Address=0x%x\r\n",mpu.getID());
    } else {
        pc.printf("MPU6050 error ID=0x%x\r\n",mpu.getID());
        while(1) {
            cpu=1;
            wait(0.5);//1秒に0.5秒点灯...加速度センサーエラー
            cpu=0;
            wait(0.5);
        }
    }
    wait(0.1);
    nrf.powerUp();
    nrf.setRxAddress(0xe7e7e7e5a4);
    nrf.setTxAddress(0xe7e7e7e7e6);
    nrf.setAirDataRate(250);
    pc.printf( "%dMHz %ddbm %dKbps ",  nrf.getRfFrequency(),nrf.getRfOutputPower(),nrf.getAirDataRate());
    pc.printf("Tx:0x%010llX ",nrf.getTxAddress());
    pc.printf("Rx:0x%010llX \r\n",nrf.getRxAddress());
    nrf.setTransferSize(4);
    nrf.setReceiveMode();
    nrf.enable();
    bat=batin.read();
    bat=bat*12.65f + 0.2f;
    txdata[0]=kx+128;
    txdata[1]=ky+128;
    txdata[2]=bat*20;
    txdata[3]=1;
    nrf.write( NRF24L01P_PIPE_P0, txdata, 4 );//初期値送信
    cpu=0;
    wait(0.2);
    mpu.start();
    bat=batin.read();
    bat=bat*12.65f + 0.2f;
    pc.printf("Battery %4.2fV\r\n",bat);
    if(bat<6.4f) {
        enable=0;
        pc.printf("Battery is low!\r\n");
    }
    txdata[0]=kx+128;
    txdata[1]=ky+128;
    txdata[2]=bat*20;
    txdata[3]=1;
    nrf.write( NRF24L01P_PIPE_P0, txdata, 4 );
    wait(0.5);
    cpu=1;
    while(i<100) {//ジャイロセンサーのオフセット測定
        mpu.read(&gxin,&gyin,&gzin,&axin,&ayin,&azin);
        gx_offset=gx_offset+gxin;
        gy_offset=gy_offset+gyin;
        gz_offset=gz_offset+gzin;
        times++;
        if(times==10) {
            cpu=!cpu;
            times=0;
        }
        i++;
        wait(0.01);
    }
    i=0;
    gx_offset=gx_offset/100;//ジャイロセンサーのオフセット計算
    gy_offset=gy_offset/100;
    gz_offset=gz_offset/100;
    cpu=0;
    //gx_offset=-2.63;
    //gy_offset=-0.71;
    pc.printf("gyro offset %.2f %.2f %.2f\r\n",gx_offset,gy_offset,gz_offset);
    wait(2);
    sampler.attach_us(&sample,samp*1000000);//ループ開始
    while(1) {
        wait(0.01);
    }
}