四轴
Dependencies: BufferedSerial SoftPWM mbed send
Fork of send_copy by
main.cpp
- Committer:
- accelerator225
- Date:
- 2017-12-10
- Revision:
- 5:683542d82dd7
- Parent:
- 4:876bfa91934c
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; } } }