![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
四轴
Dependencies: BufferedSerial SoftPWM mbed send
Fork of send_copy by
Diff: main.cpp
- Revision:
- 5:683542d82dd7
- Parent:
- 4:876bfa91934c
--- a/main.cpp Wed Nov 08 06:24:17 2017 +0000 +++ b/main.cpp Sun Dec 10 02:19:41 2017 +0000 @@ -1,99 +1,125 @@ #include "mbed.h" #include "nRF24L01P.h" #include "SoftPWM.h" -#define JY901transfersize 11 -#define TRANSFER_SIZE 11 +#define TRANSFER_SIZE 12 #include"JY901.h" #define pwm_period 1 -int flagg = 0; - -char targetdata[3]; -char receivedata[4]; -float change[4]; -int receivedatacnt = 0; -SoftPWM PWM1(PA_2),PWM2(PA_3),PWM3(PA_6),PWM4(PA_7),PWM5(PB_0),PWM6(PB_1); +#define maxroll 15 +#define maxpitch 15 +#define maxyaw 15 +#define maxAz 0.98 -int rollx=0,rolly=0,rollz=0,drx,dry,drz,ax,ay,az; -float kp,ki,kd; -void pwm_init() -{ - PWM1.period_ms(pwm_period); - PWM2.period_ms(pwm_period); - PWM3.period_ms(pwm_period); - PWM4.period_ms(pwm_period); - PWM5.period_ms(pwm_period); - PWM6.period_ms(pwm_period); - PWM1=0.0;PWM2=0.0;PWM3=0.0;PWM4=0.0;PWM5=0.0;PWM6=0.0; -} +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]; -JY901 _JY901(PA_9,PA_10); -//nRF24L01P my_nrf24l01p(PB_15, PB_14, PB_13, PC_1, PC_2, PC_0); -char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE]; - int JY901cnt = 0; - int txDataCnt = 0; - int rxDataCnt = 0; -void transferdata(){ +void flag_change(){flag=1;} +void pid_control(){ _JY901.receiveData(); - float acc[3]; - _JY901.getAttitude(acc[0], acc[1], acc[2]); - PWM1=0.2;//0.13+0.13*(float)rollx/32767.0; - PWM2=0.4;//0.13-0.13*(float)rolly/32767.0; - PWM3=0.1-0.1*acc[0]/180; + 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占空比 } - -int PIDcontrol(char *targetdata,char *data,float *change){ - return 1; -} - -void update_roll() +void init() { - // char *s; - /*char data[JY901transfersize]; - char flag =0,flag2 = 0; - int q=1; - while(q < 1000){ - q++; - switch(flag){ + 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: - flag2=JY901.getc(); - if(flag2==0x55) flag++; + if (updata_target()==1) {state=1; time.attach(&pid_control,0.01); } break; + case 1: - flag2=JY901.getc(); - if(flag2 == 0x53){ - flag++; - data[0] = 0x55; - data[1] = flag2; - } - else flag--; + if (updata_target()==2) state=2; break; + case 2: - for(int i = 2;i != JY901transfersize;i++) - data[i] = JY901.getc(); - q = 1001; + break; - } - }*/ - - // rollx+=drx; - // rolly+=dry; - //rollz+=drz; - // sprintf(s,"%f",rollx*180/32767); - // my_nrf24l01p.write( NRF24L01P_PIPE_P0,s,11); -} -int main() { - // my_nrf24l01p.powerUp(); - // my_nrf24l01p.setTransferSize( 11); - // my_nrf24l01p.setReceiveMode(); - // my_nrf24l01p.enable(); - - Ticker time; - time.attach(&transferdata,0.1); - while(1){ - - // pc.printf("succes"); - - + } } }