File content as of revision 5:683542d82dd7:

#include "mbed.h"
#include "nRF24L01P.h"
#include "SoftPWM.h"
#define TRANSFER_SIZE   12
#include"JY901.h"
#define pwm_period 1
#define maxroll 15
#define maxpitch 15
#define maxyaw 15
#define maxAz 0.98

char flag=0;//PID更新标志
SoftPWM PWM1(PA_2);
PwmOut PWM2(PA_3),PWM3(PA_6),PWM4(PA_7),PWM5(PB_0),PWM6(PB_1);//六路PWM输出接口（PA_2mbed不支持PWM输出 只能采用软件pwm）
float target_roll[3]={0,0,0},p_z=0.5;//目标
float err_old[3]={0,0,0};//记录前一个的误差值
float kp1=0.0015,ki1=5e-5,kd1=4e-2,
      kp2=0,ki2=0,kd2=0,
      kp0=0.001,ki0=5e-5,kd0=6e-2;//pid参数设置
float I[3]={0,0,0};//积分值
JY901 _JY901(PA_9,PA_10);//JY901 接口 （波特率为115200）
nRF24L01P my_nrf24l01p(PB_15, PB_14, PB_13, PC_1, PC_2, PC_0);//nrf模块
char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE];

void flag_change(){flag=1;}
void pid_control(){
    _JY901.receiveData();
    float roll[3],P1,P2,P3,P4,P5,P6;
    _JY901.getAttitude(roll[0], roll[1], roll[2]);
    if(roll[0]>180) roll[0]-=360;
    if(roll[1]>180) roll[1]-=360;
    if(roll[2]>180) roll[2]-=360;//更新数据
    
    P1=p_z;P2=p_z;P3=p_z;P4=p_z;P5=p_z;P6=p_z;
    float err[3];
    float pi1,pi2,pi0;
    err[0]=target_roll[0]-roll[0];  
    err[1]=target_roll[1]-roll[1];
    err[2]=target_roll[2]-roll[2];
    
    I[0]+=err[0];
    I[1]+=err[1]; 
    I[2]+=err[2];

    pi0=err[0]*kp0+ki0*I[0]+kd0*(err[0]-err_old[0]);
    pi1=err[1]*kp1+ki1*I[1]+kd1*(err[1]-err_old[1]);
    pi2=err[2]*kp2+ki2*I[2]+kd2*(err[2]-err_old[2]);//计算pid
    
    err_old[1]=err[1];
    err_old[2]=err[2];
    err_old[0]=err[0];
    
    if(pi1>0.25) pi1=0.25;//pid限幅
    if(pi1<-0.25) pi1=-0.25;
    if(pi0>0.25) pi0=0.25;
    if(pi0<-0.25) pi0=-0.25;
    if(pi2>0.25) pi2=0.25;
    if(pi2<-0.25) pi2=-0.25;
     
    P3+=(-pi1-pi2);
    P6+=(pi1+pi2);//计算电机pwm占空比
    P1+=(pi0-0.5*pi2);
    P2+=(pi0+0.5*pi2);
    P4+=(-pi0+0.5*pi2);
    P5+=(-pi0-0.5*pi2);
    
    PWM3=P3;PWM6=P6;
   // if(P1<=0.001) PWM1=0.001;else PWM1=P1;
   //PWM2=P2;PWM4=P4;PWM5=P5;//更新pwm占空比
}
void init()
{
    my_nrf24l01p.powerUp();  
    my_nrf24l01p.setTransferSize(TRANSFER_SIZE);
    my_nrf24l01p.setReceiveMode();
    my_nrf24l01p.enable();  
    PWM1=0.0;PWM2=0.0;PWM3=0.0;PWM4=0.0;PWM5=0.0;PWM6=0.0;    
}
int updata_target()
{
   switch (rxData[0]){
       case 0x50:
        return 1;
      // break;
       case 0x51:
        my_nrf24l01p.read( NRF24L01P_PIPE_P0, rxData, sizeof( rxData ) );
        target_roll[0] = (float)(((int)rxData[1]*256+(int)rxData[2])-32768)*(float)maxroll/65536.0f;
      //  Az =(float) ((int)rxData[3]*256+(int)rxData[4]-32768)*(float)maxAz/65536.0f;
        target_roll[1] = (float)(((int)rxData[5]*256+(int)rxData[6])-32768)*(float)maxpitch/65536.0f;
        target_roll[2] = (float)(((int)rxData[7]*256+(int)rxData[8])-32768)*(float)maxyaw/65536.0f;
        return -1;
     //  break;
       case 0x52:
        return 2;
      // break;
       }     
        return 0;
}
int state=0;
int main() { 
    init();
    Ticker time;
    wait(1);
  // time.attach(&pid_control,0.01);
    while(1){
    //if(flag){flag=0;}

        if(my_nrf24l01p.readable()) my_nrf24l01p.read(NRF24L01P_PIPE_P0,rxData,sizeof(rxData));
        switch(state)
        {
            case 0:
            if (updata_target()==1) {state=1; time.attach(&pid_control,0.01); }
            break;
            
            case 1:
            if (updata_target()==2) state=2;
            break;
            
            case 2:
            
            break;
        }
    } 
       
}