wan zhouquan / Mbed 2 deprecated fly_v1

fly_v1

Dependencies:   BufferedSerial SoftPWM mbed send

Fork of withbufferserial_nrftst by aurora moon

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?

UserRevisionLine numberNew 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