wan zhouquan
fly_v1
Dependencies: BufferedSerial SoftPWM mbed send
Fork of
withbufferserial_nrftst
by 
aurora moon
Diff: main.cpp
- Revision:
- 6:6e0f02edb404
- Parent:
- 5:683542d82dd7
--- a/main.cpp Sun Dec 10 02:19:41 2017 +0000
+++ b/main.cpp Sun Dec 24 03:15:15 2017 +0000
@@ -1,28 +1,43 @@
#include "mbed.h"
#include "nRF24L01P.h"
#include "SoftPWM.h"
+#include"JY901.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
+#define fail_max 10 //最大重连次数
+
+#define maxroll 10
+#define maxpitch 10
+#define maxyaw 10//遥控器调节的最大值
-char flag=0;//PID更新标志
+#define maxp_z 0.5
+#define maxpi0 0.3
+#define maxpi1 0.25
+#define maxpi2 0.2//参数设定需满足 0.33*maxp_z+maxpi1+maxpi2<0.5 ;maxp_z+0.5*maxpi2+maxpi0<0.5
+#define p_z_max 0.3f
+#define p_z_min -0.2
+#define I0_INIT 0.0f
+#define I1_INIT 0.0f
+
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;//目标
+PwmOut PWM2(PA_3),PWM3(PA_6),PWM4(PA_7),PWM5(PB_0),PWM6(PB_1);//六路PWM输出接口
+//PS:PA_2 mbed不支持PWM输出 与USART3的串口时钟初始化冲突 只能采用软件pwm，其原有库有一定的问题,应用此方法后不应该采用中断处理时间较长的中断）
+JY901 _JY901(PA_9,PA_10);//JY901 接口 （波特率为115200，在bufferserial库中更改！）
+nRF24L01P my_nrf24l01p(PB_15, PB_14, PB_13, PC_1, PC_2, PC_0);//nrf模块，nrf可能会由于电压波动断电，需要软件重启
+
+int state=0;//状态标志
+int pid_flag=0;//pid标志位
+int fail_num=0;//nrf通讯失败计数
+float target_roll[3]={0,0,-30},p_z=p_z_min,target_roll_attach[3]={0,0,0},p_z_attach=0;//目标
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];
+float I[3]={I0_INIT,I1_INIT,0};//积分值
+float kp1=0.004,ki1=8e-5,kd1=0.08,
+ kp2=-0.002,ki2=0,kd2=-0.12,
+ kp0=0.005,ki0=1e-4,kd0=0.10;//pid参数设置
+char rxData[TRANSFER_SIZE];
-void flag_change(){flag=1;}
+void _pid_flag(){pid_flag=1;}//定时控制pid
+
void pid_control(){
_JY901.receiveData();
float roll[3],P1,P2,P3,P4,P5,P6;
@@ -31,7 +46,28 @@
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;
+
+ target_roll_attach[2] = (float)(((int)rxData[1]*256+(int)rxData[2])-32768);
+ if(state!=3) p_z_attach =65536.0f-(float)((int)rxData[3]*256+(int)rxData[4]);
+ target_roll_attach[1] = (float)(((int)rxData[5]*256+(int)rxData[6])-32768);
+ target_roll_attach[0] = (float)(((int)rxData[7]*256+(int)rxData[8])-32768);
+
+ target_roll[0]=maxroll*target_roll_attach[0]/32767.0f;
+ target_roll[1]=maxpitch*target_roll_attach[1]/32767.0f;
+ target_roll[2]=maxyaw*target_roll_attach[2]/32767.0f;
+ if(state!=3) p_z=p_z_min+(p_z_attach/65532.0f)*(p_z_max-p_z_min);
+ else p_z+=p_z_attach*1.5e-8;
+
+ /* if (target_roll[0]>maxroll)target_roll[0]=maxroll;
+ if (target_roll[0]<-maxroll)target_roll[0]=-maxroll;
+ if (target_roll[1]>maxpitch)target_roll[1]=maxpitch;
+ if (target_roll[1]<-maxpitch)target_roll[1]=-maxpitch;
+ if (target_roll[2]>maxyaw)target_roll[2]=maxyaw;
+ if (target_roll[2]<-maxyaw)target_roll[2]=-maxyaw;*/
+ if (p_z>maxp_z) p_z=maxp_z;
+ if (p_z<-maxp_z) p_z=-maxp_z;//动力限幅
+
+ P1=0.5;P2=0.5;P3=0.5;P4=0.5;P5=0.5;P6=0.5;
float err[3];
float pi1,pi2,pi0;
err[0]=target_roll[0]-roll[0];
@@ -41,7 +77,14 @@
I[0]+=err[0];
I[1]+=err[1];
I[2]+=err[2];
-
+
+ if(I[0]>0.2/ki0) I[0]=0.2/ki0;
+ if(I[1]>0.2/ki1) I[1]=0.2/ki1;
+ if(I[2]>0.2/ki2) I[2]=0.2/ki2;
+ if(I[0]<-0.2/ki0) I[0]=-0.2/ki0;
+ if(I[1]<-0.2/ki1) I[1]=-0.2/ki1;
+ if(I[2]<-0.2/ki2) I[2]=-0.2/ki2;//积分项限幅防止长时间积分无法回调
+
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
@@ -50,77 +93,126 @@
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;
+ if(pi1>maxpi1) pi1=maxpi1;//pid限幅
+ if(pi1<-maxpi1) pi1=-maxpi1;
+ if(pi0>maxpi0) pi0=maxpi0;
+ if(pi0<-maxpi0) pi0=-maxpi0;
+ if(pi2>maxpi2) pi2=maxpi2;
+ if(pi2<-maxpi2) pi2=-maxpi2;
- 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);
+ P3+=(p_z/3-pi1-pi2);
+ P6+=(p_z/3+pi1+pi2);
+ P1+=(p_z+pi0-0.5*pi2);
+ P2+=(p_z+pi0+0.5*pi2);
+ P4+=(p_z+-pi0+0.5*pi2);
+ P5+=(p_z+-pi0-0.5*pi2);//计算电机pwm占空比
PWM3=P3;PWM6=P6;
- // if(P1<=0.001) PWM1=0.001;else PWM1=P1;
- //PWM2=P2;PWM4=P4;PWM5=P5;//更新pwm占空比
+ if(P1<=0.01) PWM1=0.01;else PWM1=P1;
+ PWM2=P2;
+ PWM4=P4;PWM5=P5;//更新pwm占空比
}
-void init()
+
+void nrf_restart()
{
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()
+ my_nrf24l01p.enable();
+}//nrf重启动
+
+int get_command()
{
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;
+
+ case 0x53:
+ return 3;
+ }
+ return 0;
}
-int state=0;
-int main() {
- init();
+
+void state_config()
+{
+ switch(state)
+ {
+ case 0://待机状态等待启动命令
+ if (get_command()==1) {p_z=p_z_min;p_z_attach=0;state=1;}
+ break;
+
+ case 1://启动状态，等待缓降(0x53)或急停命令(0x52)
+ int u_flag=get_command();
+ if (u_flag==3) {state=3;break;}
+
+ if (u_flag==2) {state=2;break;}
+
+ break;
+
+ case 2://急停状态，急停后进入待机状态
+ PWM3=0;PWM6=0;PWM4=0;PWM5=0;PWM1=0;PWM2=0;
+ state=0;
+ target_roll[0]=0;
+ target_roll[1]=0;
+ target_roll[2]=0;
+ target_roll_attach[0]=0;
+ target_roll_attach[1]=0;
+ target_roll_attach[2]=0;
+ I[0]=I0_INIT;
+ I[1]=I1_INIT;I[2]=0;
+ // p_z_attach=-2e5;
+ break;
+
+ case 3://缓降状态，等待急停命令，缓降更改目标为0，匀减速降落。
+ if (get_command()==2) state=2;
+ target_roll[0]=0;
+ target_roll[1]=0;
+ target_roll[2]=0;
+ target_roll_attach[0]=0;
+ target_roll_attach[1]=0;
+ target_roll_attach[2]=0;
+ p_z_attach=-2e5;
+ break;
+ }
+}
+void pwm_init(){
+ //PWM1.period_us(100);
+ PWM2.period_ms(1);
+ PWM3.period_ms(1);
+ PWM4.period_ms(1);
+ PWM5.period_ms(1);
+ PWM6.period_ms(1);
+ PWM1=0.0;PWM2=0.0;PWM3=0.0;PWM4=0.0;PWM5=0.0;PWM6=0.0;
+}
+
+int main() {
+ pwm_init();
+ nrf_restart();
Ticker time;
- wait(1);
- // time.attach(&pid_control,0.01);
+ time.attach(&_pid_flag,0.01);
while(1){
- //if(flag){flag=0;}
-
- if(my_nrf24l01p.readable()) my_nrf24l01p.read(NRF24L01P_PIPE_P0,rxData,sizeof(rxData));
+ if(my_nrf24l01p.readable()) {
+ my_nrf24l01p.read(NRF24L01P_PIPE_P0,rxData,sizeof(rxData));
+ state_config();
+ }
+ else {
+ if (fail_num==fail_max) {nrf_restart();fail_num=0;}
+ else fail_num++;
+ }//接收遥控器指令,一定次数没收到指令自动重新启动nrf模块
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;
+ if (pid_flag) {pid_control();pid_flag=0;}
break;
-
- case 2:
-
+ case 3:
+ if (pid_flag) {pid_control();pid_flag=0;}
break;
}
- }
-
+ }
}