main.cpp

00001 #include "mbed.h"
00002 #include "nRF24L01P.h"
00003 #include "SoftPWM.h"
00004 #define TRANSFER_SIZE   12
00005 #include"JY901.h"
00006 #define pwm_period 1
00007 #define maxroll 15
00008 #define maxpitch 15
00009 #define maxyaw 15
00010 #define maxAz 0.98
00011 
00012 char flag=0;//PID更新标志
00013 SoftPWM PWM1(PA_2);
00014 PwmOut PWM2(PA_3),PWM3(PA_6),PWM4(PA_7),PWM5(PB_0),PWM6(PB_1);//六路PWM输出接口（PA_2mbed不支持PWM输出 只能采用软件pwm）
00015 float target_roll[3]={0,0,0},p_z=0.5;//目标
00016 float err_old[3]={0,0,0};//记录前一个的误差值
00017 float kp1=0.0015,ki1=5e-5,kd1=4e-2,
00018       kp2=0,ki2=0,kd2=0,
00019       kp0=0.001,ki0=5e-5,kd0=6e-2;//pid参数设置
00020 float I[3]={0,0,0};//积分值
00021 JY901 _JY901(PA_9,PA_10);//JY901 接口 （波特率为115200）
00022 nRF24L01P my_nrf24l01p(PB_15, PB_14, PB_13, PC_1, PC_2, PC_0);//nrf模块
00023 char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE];
00024 
00025 void flag_change(){flag=1;}
00026 void pid_control(){
00027     _JY901.receiveData();
00028     float roll[3],P1,P2,P3,P4,P5,P6;
00029     _JY901.getAttitude(roll[0], roll[1], roll[2]);
00030     if(roll[0]>180) roll[0]-=360;
00031     if(roll[1]>180) roll[1]-=360;
00032     if(roll[2]>180) roll[2]-=360;//更新数据
00033     
00034     P1=p_z;P2=p_z;P3=p_z;P4=p_z;P5=p_z;P6=p_z;
00035     float err[3];
00036     float pi1,pi2,pi0;
00037     err[0]=target_roll[0]-roll[0];  
00038     err[1]=target_roll[1]-roll[1];
00039     err[2]=target_roll[2]-roll[2];
00040     
00041     I[0]+=err[0];
00042     I[1]+=err[1]; 
00043     I[2]+=err[2];
00044 
00045     pi0=err[0]*kp0+ki0*I[0]+kd0*(err[0]-err_old[0]);
00046     pi1=err[1]*kp1+ki1*I[1]+kd1*(err[1]-err_old[1]);
00047     pi2=err[2]*kp2+ki2*I[2]+kd2*(err[2]-err_old[2]);//计算pid
00048     
00049     err_old[1]=err[1];
00050     err_old[2]=err[2];
00051     err_old[0]=err[0];
00052     
00053     if(pi1>0.25) pi1=0.25;//pid限幅
00054     if(pi1<-0.25) pi1=-0.25;
00055     if(pi0>0.25) pi0=0.25;
00056     if(pi0<-0.25) pi0=-0.25;
00057     if(pi2>0.25) pi2=0.25;
00058     if(pi2<-0.25) pi2=-0.25;
00059      
00060     P3+=(-pi1-pi2);
00061     P6+=(pi1+pi2);//计算电机pwm占空比
00062     P1+=(pi0-0.5*pi2);
00063     P2+=(pi0+0.5*pi2);
00064     P4+=(-pi0+0.5*pi2);
00065     P5+=(-pi0-0.5*pi2);
00066     
00067     PWM3=P3;PWM6=P6;
00068    // if(P1<=0.001) PWM1=0.001;else PWM1=P1;
00069    //PWM2=P2;PWM4=P4;PWM5=P5;//更新pwm占空比
00070 }
00071 void init()
00072 {
00073     my_nrf24l01p.powerUp();  
00074     my_nrf24l01p.setTransferSize(TRANSFER_SIZE);
00075     my_nrf24l01p.setReceiveMode();
00076     my_nrf24l01p.enable();  
00077     PWM1=0.0;PWM2=0.0;PWM3=0.0;PWM4=0.0;PWM5=0.0;PWM6=0.0;    
00078 }
00079 int updata_target()
00080 {
00081    switch (rxData[0]){
00082        case 0x50:
00083         return 1;
00084       // break;
00085        case 0x51:
00086         my_nrf24l01p.read( NRF24L01P_PIPE_P0, rxData, sizeof( rxData ) );
00087         target_roll[0] = (float)(((int)rxData[1]*256+(int)rxData[2])-32768)*(float)maxroll/65536.0f;
00088       //  Az =(float) ((int)rxData[3]*256+(int)rxData[4]-32768)*(float)maxAz/65536.0f;
00089         target_roll[1] = (float)(((int)rxData[5]*256+(int)rxData[6])-32768)*(float)maxpitch/65536.0f;
00090         target_roll[2] = (float)(((int)rxData[7]*256+(int)rxData[8])-32768)*(float)maxyaw/65536.0f;
00091         return -1;
00092      //  break;
00093        case 0x52:
00094         return 2;
00095       // break;
00096        }     
00097         return 0;
00098 }
00099 int state=0;
00100 int main() { 
00101     init();
00102     Ticker time;
00103     wait(1);
00104   // time.attach(&pid_control,0.01);
00105     while(1){
00106     //if(flag){flag=0;}
00107 
00108         if(my_nrf24l01p.readable()) my_nrf24l01p.read(NRF24L01P_PIPE_P0,rxData,sizeof(rxData));
00109         switch(state)
00110         {
00111             case 0:
00112             if (updata_target()==1) {state=1; time.attach(&pid_control,0.01); }
00113             break;
00114             
00115             case 1:
00116             if (updata_target()==2) state=2;
00117             break;
00118             
00119             case 2:
00120             
00121             break;
00122         }
00123     } 
00124        
00125 }
00126