![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
fly_v1
Dependencies: BufferedSerial SoftPWM mbed send
Fork of withbufferserial_nrftst by
main.cpp@6:6e0f02edb404, 2017-12-24 (annotated)
- Committer:
- wanzq
- Date:
- Sun Dec 24 03:15:15 2017 +0000
- Revision:
- 6:6e0f02edb404
- Parent:
- 5:683542d82dd7
fly pro
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Owen | 0:a51a6e7da590 | 1 | #include "mbed.h" |
Owen | 0:a51a6e7da590 | 2 | #include "nRF24L01P.h" |
accelerator225 | 4:876bfa91934c | 3 | #include "SoftPWM.h" |
wanzq | 6:6e0f02edb404 | 4 | #include"JY901.h" |
wanzq | 6:6e0f02edb404 | 5 | |
accelerator225 | 5:683542d82dd7 | 6 | #define TRANSFER_SIZE 12 |
wanzq | 6:6e0f02edb404 | 7 | #define fail_max 10 //最大重连次数 |
wanzq | 6:6e0f02edb404 | 8 | |
wanzq | 6:6e0f02edb404 | 9 | #define maxroll 10 |
wanzq | 6:6e0f02edb404 | 10 | #define maxpitch 10 |
wanzq | 6:6e0f02edb404 | 11 | #define maxyaw 10//遥控器调节的最大值 |
Owen | 0:a51a6e7da590 | 12 | |
wanzq | 6:6e0f02edb404 | 13 | #define maxp_z 0.5 |
wanzq | 6:6e0f02edb404 | 14 | #define maxpi0 0.3 |
wanzq | 6:6e0f02edb404 | 15 | #define maxpi1 0.25 |
wanzq | 6:6e0f02edb404 | 16 | #define maxpi2 0.2//参数设定需满足 0.33*maxp_z+maxpi1+maxpi2<0.5 ;maxp_z+0.5*maxpi2+maxpi0<0.5 |
wanzq | 6:6e0f02edb404 | 17 | #define p_z_max 0.3f |
wanzq | 6:6e0f02edb404 | 18 | #define p_z_min -0.2 |
wanzq | 6:6e0f02edb404 | 19 | #define I0_INIT 0.0f |
wanzq | 6:6e0f02edb404 | 20 | #define I1_INIT 0.0f |
wanzq | 6:6e0f02edb404 | 21 | |
accelerator225 | 5:683542d82dd7 | 22 | SoftPWM PWM1(PA_2); |
wanzq | 6:6e0f02edb404 | 23 | PwmOut PWM2(PA_3),PWM3(PA_6),PWM4(PA_7),PWM5(PB_0),PWM6(PB_1);//六路PWM输出接口 |
wanzq | 6:6e0f02edb404 | 24 | //PS:PA_2 mbed不支持PWM输出 与USART3的串口时钟初始化冲突 只能采用软件pwm,其原有库有一定的问题,应用此方法后不应该采用中断处理时间较长的中断) |
wanzq | 6:6e0f02edb404 | 25 | JY901 _JY901(PA_9,PA_10);//JY901 接口 (波特率为115200,在bufferserial库中更改!) |
wanzq | 6:6e0f02edb404 | 26 | nRF24L01P my_nrf24l01p(PB_15, PB_14, PB_13, PC_1, PC_2, PC_0);//nrf模块,nrf可能会由于电压波动断电,需要软件重启 |
wanzq | 6:6e0f02edb404 | 27 | |
wanzq | 6:6e0f02edb404 | 28 | int state=0;//状态标志 |
wanzq | 6:6e0f02edb404 | 29 | int pid_flag=0;//pid标志位 |
wanzq | 6:6e0f02edb404 | 30 | int fail_num=0;//nrf通讯失败计数 |
wanzq | 6:6e0f02edb404 | 31 | float target_roll[3]={0,0,-30},p_z=p_z_min,target_roll_attach[3]={0,0,0},p_z_attach=0;//目标 |
accelerator225 | 5:683542d82dd7 | 32 | float err_old[3]={0,0,0};//记录前一个的误差值 |
wanzq | 6:6e0f02edb404 | 33 | float I[3]={I0_INIT,I1_INIT,0};//积分值 |
wanzq | 6:6e0f02edb404 | 34 | float kp1=0.004,ki1=8e-5,kd1=0.08, |
wanzq | 6:6e0f02edb404 | 35 | kp2=-0.002,ki2=0,kd2=-0.12, |
wanzq | 6:6e0f02edb404 | 36 | kp0=0.005,ki0=1e-4,kd0=0.10;//pid参数设置 |
wanzq | 6:6e0f02edb404 | 37 | char rxData[TRANSFER_SIZE]; |
Owen | 0:a51a6e7da590 | 38 | |
wanzq | 6:6e0f02edb404 | 39 | void _pid_flag(){pid_flag=1;}//定时控制pid |
wanzq | 6:6e0f02edb404 | 40 | |
accelerator225 | 5:683542d82dd7 | 41 | void pid_control(){ |
accelerator225 | 4:876bfa91934c | 42 | _JY901.receiveData(); |
accelerator225 | 5:683542d82dd7 | 43 | float roll[3],P1,P2,P3,P4,P5,P6; |
accelerator225 | 5:683542d82dd7 | 44 | _JY901.getAttitude(roll[0], roll[1], roll[2]); |
accelerator225 | 5:683542d82dd7 | 45 | if(roll[0]>180) roll[0]-=360; |
accelerator225 | 5:683542d82dd7 | 46 | if(roll[1]>180) roll[1]-=360; |
accelerator225 | 5:683542d82dd7 | 47 | if(roll[2]>180) roll[2]-=360;//更新数据 |
accelerator225 | 5:683542d82dd7 | 48 | |
wanzq | 6:6e0f02edb404 | 49 | |
wanzq | 6:6e0f02edb404 | 50 | target_roll_attach[2] = (float)(((int)rxData[1]*256+(int)rxData[2])-32768); |
wanzq | 6:6e0f02edb404 | 51 | if(state!=3) p_z_attach =65536.0f-(float)((int)rxData[3]*256+(int)rxData[4]); |
wanzq | 6:6e0f02edb404 | 52 | target_roll_attach[1] = (float)(((int)rxData[5]*256+(int)rxData[6])-32768); |
wanzq | 6:6e0f02edb404 | 53 | target_roll_attach[0] = (float)(((int)rxData[7]*256+(int)rxData[8])-32768); |
wanzq | 6:6e0f02edb404 | 54 | |
wanzq | 6:6e0f02edb404 | 55 | target_roll[0]=maxroll*target_roll_attach[0]/32767.0f; |
wanzq | 6:6e0f02edb404 | 56 | target_roll[1]=maxpitch*target_roll_attach[1]/32767.0f; |
wanzq | 6:6e0f02edb404 | 57 | target_roll[2]=maxyaw*target_roll_attach[2]/32767.0f; |
wanzq | 6:6e0f02edb404 | 58 | if(state!=3) p_z=p_z_min+(p_z_attach/65532.0f)*(p_z_max-p_z_min); |
wanzq | 6:6e0f02edb404 | 59 | else p_z+=p_z_attach*1.5e-8; |
wanzq | 6:6e0f02edb404 | 60 | |
wanzq | 6:6e0f02edb404 | 61 | /* if (target_roll[0]>maxroll)target_roll[0]=maxroll; |
wanzq | 6:6e0f02edb404 | 62 | if (target_roll[0]<-maxroll)target_roll[0]=-maxroll; |
wanzq | 6:6e0f02edb404 | 63 | if (target_roll[1]>maxpitch)target_roll[1]=maxpitch; |
wanzq | 6:6e0f02edb404 | 64 | if (target_roll[1]<-maxpitch)target_roll[1]=-maxpitch; |
wanzq | 6:6e0f02edb404 | 65 | if (target_roll[2]>maxyaw)target_roll[2]=maxyaw; |
wanzq | 6:6e0f02edb404 | 66 | if (target_roll[2]<-maxyaw)target_roll[2]=-maxyaw;*/ |
wanzq | 6:6e0f02edb404 | 67 | if (p_z>maxp_z) p_z=maxp_z; |
wanzq | 6:6e0f02edb404 | 68 | if (p_z<-maxp_z) p_z=-maxp_z;//动力限幅 |
wanzq | 6:6e0f02edb404 | 69 | |
wanzq | 6:6e0f02edb404 | 70 | P1=0.5;P2=0.5;P3=0.5;P4=0.5;P5=0.5;P6=0.5; |
accelerator225 | 5:683542d82dd7 | 71 | float err[3]; |
accelerator225 | 5:683542d82dd7 | 72 | float pi1,pi2,pi0; |
accelerator225 | 5:683542d82dd7 | 73 | err[0]=target_roll[0]-roll[0]; |
accelerator225 | 5:683542d82dd7 | 74 | err[1]=target_roll[1]-roll[1]; |
accelerator225 | 5:683542d82dd7 | 75 | err[2]=target_roll[2]-roll[2]; |
accelerator225 | 5:683542d82dd7 | 76 | |
accelerator225 | 5:683542d82dd7 | 77 | I[0]+=err[0]; |
accelerator225 | 5:683542d82dd7 | 78 | I[1]+=err[1]; |
accelerator225 | 5:683542d82dd7 | 79 | I[2]+=err[2]; |
wanzq | 6:6e0f02edb404 | 80 | |
wanzq | 6:6e0f02edb404 | 81 | if(I[0]>0.2/ki0) I[0]=0.2/ki0; |
wanzq | 6:6e0f02edb404 | 82 | if(I[1]>0.2/ki1) I[1]=0.2/ki1; |
wanzq | 6:6e0f02edb404 | 83 | if(I[2]>0.2/ki2) I[2]=0.2/ki2; |
wanzq | 6:6e0f02edb404 | 84 | if(I[0]<-0.2/ki0) I[0]=-0.2/ki0; |
wanzq | 6:6e0f02edb404 | 85 | if(I[1]<-0.2/ki1) I[1]=-0.2/ki1; |
wanzq | 6:6e0f02edb404 | 86 | if(I[2]<-0.2/ki2) I[2]=-0.2/ki2;//积分项限幅防止长时间积分无法回调 |
wanzq | 6:6e0f02edb404 | 87 | |
accelerator225 | 5:683542d82dd7 | 88 | pi0=err[0]*kp0+ki0*I[0]+kd0*(err[0]-err_old[0]); |
accelerator225 | 5:683542d82dd7 | 89 | pi1=err[1]*kp1+ki1*I[1]+kd1*(err[1]-err_old[1]); |
accelerator225 | 5:683542d82dd7 | 90 | pi2=err[2]*kp2+ki2*I[2]+kd2*(err[2]-err_old[2]);//计算pid |
accelerator225 | 5:683542d82dd7 | 91 | |
accelerator225 | 5:683542d82dd7 | 92 | err_old[1]=err[1]; |
accelerator225 | 5:683542d82dd7 | 93 | err_old[2]=err[2]; |
accelerator225 | 5:683542d82dd7 | 94 | err_old[0]=err[0]; |
accelerator225 | 5:683542d82dd7 | 95 | |
wanzq | 6:6e0f02edb404 | 96 | if(pi1>maxpi1) pi1=maxpi1;//pid限幅 |
wanzq | 6:6e0f02edb404 | 97 | if(pi1<-maxpi1) pi1=-maxpi1; |
wanzq | 6:6e0f02edb404 | 98 | if(pi0>maxpi0) pi0=maxpi0; |
wanzq | 6:6e0f02edb404 | 99 | if(pi0<-maxpi0) pi0=-maxpi0; |
wanzq | 6:6e0f02edb404 | 100 | if(pi2>maxpi2) pi2=maxpi2; |
wanzq | 6:6e0f02edb404 | 101 | if(pi2<-maxpi2) pi2=-maxpi2; |
accelerator225 | 5:683542d82dd7 | 102 | |
wanzq | 6:6e0f02edb404 | 103 | P3+=(p_z/3-pi1-pi2); |
wanzq | 6:6e0f02edb404 | 104 | P6+=(p_z/3+pi1+pi2); |
wanzq | 6:6e0f02edb404 | 105 | P1+=(p_z+pi0-0.5*pi2); |
wanzq | 6:6e0f02edb404 | 106 | P2+=(p_z+pi0+0.5*pi2); |
wanzq | 6:6e0f02edb404 | 107 | P4+=(p_z+-pi0+0.5*pi2); |
wanzq | 6:6e0f02edb404 | 108 | P5+=(p_z+-pi0-0.5*pi2);//计算电机pwm占空比 |
accelerator225 | 5:683542d82dd7 | 109 | |
accelerator225 | 5:683542d82dd7 | 110 | PWM3=P3;PWM6=P6; |
wanzq | 6:6e0f02edb404 | 111 | if(P1<=0.01) PWM1=0.01;else PWM1=P1; |
wanzq | 6:6e0f02edb404 | 112 | PWM2=P2; |
wanzq | 6:6e0f02edb404 | 113 | PWM4=P4;PWM5=P5;//更新pwm占空比 |
accelerator225 | 4:876bfa91934c | 114 | } |
wanzq | 6:6e0f02edb404 | 115 | |
wanzq | 6:6e0f02edb404 | 116 | void nrf_restart() |
accelerator225 | 4:876bfa91934c | 117 | { |
accelerator225 | 5:683542d82dd7 | 118 | my_nrf24l01p.powerUp(); |
accelerator225 | 5:683542d82dd7 | 119 | my_nrf24l01p.setTransferSize(TRANSFER_SIZE); |
accelerator225 | 5:683542d82dd7 | 120 | my_nrf24l01p.setReceiveMode(); |
wanzq | 6:6e0f02edb404 | 121 | my_nrf24l01p.enable(); |
wanzq | 6:6e0f02edb404 | 122 | }//nrf重启动 |
wanzq | 6:6e0f02edb404 | 123 | |
wanzq | 6:6e0f02edb404 | 124 | int get_command() |
accelerator225 | 5:683542d82dd7 | 125 | { |
accelerator225 | 5:683542d82dd7 | 126 | switch (rxData[0]){ |
accelerator225 | 5:683542d82dd7 | 127 | case 0x50: |
accelerator225 | 5:683542d82dd7 | 128 | return 1; |
wanzq | 6:6e0f02edb404 | 129 | |
accelerator225 | 5:683542d82dd7 | 130 | case 0x52: |
accelerator225 | 5:683542d82dd7 | 131 | return 2; |
wanzq | 6:6e0f02edb404 | 132 | |
wanzq | 6:6e0f02edb404 | 133 | case 0x53: |
wanzq | 6:6e0f02edb404 | 134 | return 3; |
wanzq | 6:6e0f02edb404 | 135 | } |
wanzq | 6:6e0f02edb404 | 136 | return 0; |
accelerator225 | 5:683542d82dd7 | 137 | } |
wanzq | 6:6e0f02edb404 | 138 | |
wanzq | 6:6e0f02edb404 | 139 | void state_config() |
wanzq | 6:6e0f02edb404 | 140 | { |
wanzq | 6:6e0f02edb404 | 141 | switch(state) |
wanzq | 6:6e0f02edb404 | 142 | { |
wanzq | 6:6e0f02edb404 | 143 | case 0://待机状态等待启动命令 |
wanzq | 6:6e0f02edb404 | 144 | if (get_command()==1) {p_z=p_z_min;p_z_attach=0;state=1;} |
wanzq | 6:6e0f02edb404 | 145 | break; |
wanzq | 6:6e0f02edb404 | 146 | |
wanzq | 6:6e0f02edb404 | 147 | case 1://启动状态,等待缓降(0x53)或急停命令(0x52) |
wanzq | 6:6e0f02edb404 | 148 | int u_flag=get_command(); |
wanzq | 6:6e0f02edb404 | 149 | if (u_flag==3) {state=3;break;} |
wanzq | 6:6e0f02edb404 | 150 | |
wanzq | 6:6e0f02edb404 | 151 | if (u_flag==2) {state=2;break;} |
wanzq | 6:6e0f02edb404 | 152 | |
wanzq | 6:6e0f02edb404 | 153 | break; |
wanzq | 6:6e0f02edb404 | 154 | |
wanzq | 6:6e0f02edb404 | 155 | case 2://急停状态,急停后进入待机状态 |
wanzq | 6:6e0f02edb404 | 156 | PWM3=0;PWM6=0;PWM4=0;PWM5=0;PWM1=0;PWM2=0; |
wanzq | 6:6e0f02edb404 | 157 | state=0; |
wanzq | 6:6e0f02edb404 | 158 | target_roll[0]=0; |
wanzq | 6:6e0f02edb404 | 159 | target_roll[1]=0; |
wanzq | 6:6e0f02edb404 | 160 | target_roll[2]=0; |
wanzq | 6:6e0f02edb404 | 161 | target_roll_attach[0]=0; |
wanzq | 6:6e0f02edb404 | 162 | target_roll_attach[1]=0; |
wanzq | 6:6e0f02edb404 | 163 | target_roll_attach[2]=0; |
wanzq | 6:6e0f02edb404 | 164 | I[0]=I0_INIT; |
wanzq | 6:6e0f02edb404 | 165 | I[1]=I1_INIT;I[2]=0; |
wanzq | 6:6e0f02edb404 | 166 | // p_z_attach=-2e5; |
wanzq | 6:6e0f02edb404 | 167 | break; |
wanzq | 6:6e0f02edb404 | 168 | |
wanzq | 6:6e0f02edb404 | 169 | case 3://缓降状态,等待急停命令,缓降更改目标为0,匀减速降落。 |
wanzq | 6:6e0f02edb404 | 170 | if (get_command()==2) state=2; |
wanzq | 6:6e0f02edb404 | 171 | target_roll[0]=0; |
wanzq | 6:6e0f02edb404 | 172 | target_roll[1]=0; |
wanzq | 6:6e0f02edb404 | 173 | target_roll[2]=0; |
wanzq | 6:6e0f02edb404 | 174 | target_roll_attach[0]=0; |
wanzq | 6:6e0f02edb404 | 175 | target_roll_attach[1]=0; |
wanzq | 6:6e0f02edb404 | 176 | target_roll_attach[2]=0; |
wanzq | 6:6e0f02edb404 | 177 | p_z_attach=-2e5; |
wanzq | 6:6e0f02edb404 | 178 | break; |
wanzq | 6:6e0f02edb404 | 179 | } |
wanzq | 6:6e0f02edb404 | 180 | } |
wanzq | 6:6e0f02edb404 | 181 | void pwm_init(){ |
wanzq | 6:6e0f02edb404 | 182 | //PWM1.period_us(100); |
wanzq | 6:6e0f02edb404 | 183 | PWM2.period_ms(1); |
wanzq | 6:6e0f02edb404 | 184 | PWM3.period_ms(1); |
wanzq | 6:6e0f02edb404 | 185 | PWM4.period_ms(1); |
wanzq | 6:6e0f02edb404 | 186 | PWM5.period_ms(1); |
wanzq | 6:6e0f02edb404 | 187 | PWM6.period_ms(1); |
wanzq | 6:6e0f02edb404 | 188 | PWM1=0.0;PWM2=0.0;PWM3=0.0;PWM4=0.0;PWM5=0.0;PWM6=0.0; |
wanzq | 6:6e0f02edb404 | 189 | } |
wanzq | 6:6e0f02edb404 | 190 | |
wanzq | 6:6e0f02edb404 | 191 | int main() { |
wanzq | 6:6e0f02edb404 | 192 | pwm_init(); |
wanzq | 6:6e0f02edb404 | 193 | nrf_restart(); |
accelerator225 | 5:683542d82dd7 | 194 | Ticker time; |
wanzq | 6:6e0f02edb404 | 195 | time.attach(&_pid_flag,0.01); |
accelerator225 | 5:683542d82dd7 | 196 | while(1){ |
wanzq | 6:6e0f02edb404 | 197 | if(my_nrf24l01p.readable()) { |
wanzq | 6:6e0f02edb404 | 198 | my_nrf24l01p.read(NRF24L01P_PIPE_P0,rxData,sizeof(rxData)); |
wanzq | 6:6e0f02edb404 | 199 | state_config(); |
wanzq | 6:6e0f02edb404 | 200 | } |
wanzq | 6:6e0f02edb404 | 201 | else { |
wanzq | 6:6e0f02edb404 | 202 | if (fail_num==fail_max) {nrf_restart();fail_num=0;} |
wanzq | 6:6e0f02edb404 | 203 | else fail_num++; |
wanzq | 6:6e0f02edb404 | 204 | }//接收遥控器指令,一定次数没收到指令自动重新启动nrf模块 |
accelerator225 | 5:683542d82dd7 | 205 | switch(state) |
accelerator225 | 5:683542d82dd7 | 206 | { |
accelerator225 | 4:876bfa91934c | 207 | case 0: |
accelerator225 | 4:876bfa91934c | 208 | break; |
accelerator225 | 4:876bfa91934c | 209 | case 1: |
wanzq | 6:6e0f02edb404 | 210 | if (pid_flag) {pid_control();pid_flag=0;} |
accelerator225 | 4:876bfa91934c | 211 | break; |
wanzq | 6:6e0f02edb404 | 212 | case 3: |
wanzq | 6:6e0f02edb404 | 213 | if (pid_flag) {pid_control();pid_flag=0;} |
accelerator225 | 4:876bfa91934c | 214 | break; |
accelerator225 | 5:683542d82dd7 | 215 | } |
wanzq | 6:6e0f02edb404 | 216 | } |
accelerator225 | 4:876bfa91934c | 217 | } |
Owen | 0:a51a6e7da590 | 218 |