fly_v1
Dependencies: BufferedSerial SoftPWM mbed send
Fork of withbufferserial_nrftst by
Revision 6:6e0f02edb404, committed 2017-12-24
- Comitter:
- wanzq
- Date:
- Sun Dec 24 03:15:15 2017 +0000
- Parent:
- 5:683542d82dd7
- Commit message:
- fly pro
Changed in this revision
--- a/JY901.cpp Sun Dec 10 02:19:41 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,93 +0,0 @@ - -#include "JY901.h" -void JY901::receiveData() -{ - char ch; - while(mod.readable()){ - ch = mod.getc(); - parseInput(&ch, 1); - } -} -void JY901::parseCmpt(int token, unsigned char* payloadBuf, int payloadLen) -{ - switch(token){ - case 0x51: - for (int i = 0; i < 3; ++i) - { - acc[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8); - acc[i] = acc[i] * 16 * 9.8 / 32768; - } - // lcd.cls(); - // lcd.printf("Ax=%d\n",int(data[0]*100)); - //pc.printf("Ax=%.2f\tAy=%.2f\tAz=%.2f\r\n", data[0], data[1], data[2]); - break; - case 0x52: - for (int i = 0; i < 3; ++i) - { - gyo[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8); - gyo[i] = gyo[i] * 2000 / 32768; - } - //pc.printf("Wx=%.2f\tWy=%.2f\tWz=%.2f\r\n", data[0], data[1], data[2]); - break; - case 0x53: - for (int i = 0; i < 3; ++i) - { - att[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8); - att[i] = att[i] * 180 / 32768; - } - // pc.printf("Roll=%.2f\tPitch=%.2f\tYaw=%.2f\r\n", data[0], data[1], data[2]); - break; - case 0x54: - for (int i = 0; i < 3; ++i) - { - mag[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8); - } - //pc.printf("Hx=%.2f\tHy=%.2f\tHz=%.2f\r\n", data[0], data[1], data[2]); - break; - } -} -void JY901::parseInput(const char* data, int len) -{ - for (int i = 0; i < len; ++i) - { - unsigned char ch = data[i], sum; - switch(state){ - case 0: - if(ch == 0x55) - state = 1; - break; - case 1: - token = ch; - if(0x51 <= token && token <= 0x54){ - payloadLen = 8; - recvLen = 0; - state = 2; - }else{ - // pc.printf("%s %x\r\n", "unknown token", token); - state = 0; - } - break; - case 2: - payloadBuf[recvLen++] = ch; - if(recvLen == payloadLen){ - state = 3; - } - break; - case 3: - sum = 0x55; - sum += token; - for (int i = 0; i < payloadLen; ++i) - { - sum += payloadBuf[i]; - } - if(sum != ch){ - // pc.printf("wrong checksum\r\n"); - }else{ - parseCmpt(token, payloadBuf, payloadLen); - //myled = !myled; - } - state = 0; - break; - } - } -} \ No newline at end of file
--- a/JY901.h Sun Dec 10 02:19:41 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,36 +0,0 @@ -#include "mbed.h" -#include "BufferedSerial.h"//波特率在这个头文件中改 -class JY901 -{ - int state, token, payloadLen, recvLen; - unsigned char payloadBuf[16]; -protected: - BufferedSerial mod; - float acc[3], gyo[3], mag[3], att[3]; - void parseCmpt(int token, unsigned char* payloadBuf, int payloadLen); - void parseInput(const char* data, int len); -public: - JY901(PinName TX, PinName RX) : mod(TX, RX){} - ~JY901() {} - void receiveData(); - void getAcc(float &x, float &y, float &z){ - x = acc[0]; - y = acc[1]; - z = acc[2]; - } - void getGyo(float &x, float &y, float &z){ - x = gyo[0]; - y = gyo[1]; - z = gyo[2]; - } - void getMag(float &x, float &y, float &z){ - x = mag[0]; - y = mag[1]; - z = mag[2]; - } - void getAttitude(float &roll, float &pitch, float &yaw){ - roll = att[0]; - pitch = att[1]; - yaw = att[2]; - } -}; \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/JY901/JY901.cpp Sun Dec 24 03:15:15 2017 +0000 @@ -0,0 +1,89 @@ +#include "JY901.h" +void JY901::receiveData() +{ + char ch; + while(mod.readable()){ + ch = mod.getc(); + parseInput(&ch, 1); + } +} +void JY901::parseCmpt(int token, unsigned char* payloadBuf, int payloadLen) +{ + switch(token){ + case 0x51: + for (int i = 0; i < 3; ++i) + { + acc[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8); + acc[i] = acc[i] * 16 * 9.8 / 32768; + } + break; + case 0x52: + for (int i = 0; i < 3; ++i) + { + gyo[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8); + gyo[i] = gyo[i] * 2000 / 32768; + } + //pc.printf("Wx=%.2f\tWy=%.2f\tWz=%.2f\r\n", data[0], data[1], data[2]); + break; + case 0x53: + for (int i = 0; i < 3; ++i) + { + att[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8); + att[i] = att[i] * 180 / 32768; + } + // pc.printf("Roll=%.2f\tPitch=%.2f\tYaw=%.2f\r\n", data[0], data[1], data[2]); + break; + case 0x54: + for (int i = 0; i < 3; ++i) + { + mag[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8); + } + //pc.printf("Hx=%.2f\tHy=%.2f\tHz=%.2f\r\n", data[0], data[1], data[2]); + break; + } +} +void JY901::parseInput(const char* data, int len) +{ + for (int i = 0; i < len; ++i) + { + unsigned char ch = data[i], sum; + switch(state){ + case 0: + if(ch == 0x55) + state = 1; + break; + case 1: + token = ch; + if(0x51 <= token && token <= 0x54){ + payloadLen = 8; + recvLen = 0; + state = 2; + }else{ + // pc.printf("%s %x\r\n", "unknown token", token); + state = 0; + } + break; + case 2: + payloadBuf[recvLen++] = ch; + if(recvLen == payloadLen){ + state = 3; + } + break; + case 3: + sum = 0x55; + sum += token; + for (int i = 0; i < payloadLen; ++i) + { + sum += payloadBuf[i]; + } + if(sum != ch){ + // pc.printf("wrong checksum\r\n"); + }else{ + parseCmpt(token, payloadBuf, payloadLen); + //myled = !myled; + } + state = 0; + break; + } + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/JY901/JY901.h Sun Dec 24 03:15:15 2017 +0000 @@ -0,0 +1,36 @@ +#include "mbed.h" +#include "BufferedSerial.h"//波特率在这个头文件中改 +class JY901 +{ + int state, token, payloadLen, recvLen; + unsigned char payloadBuf[16]; +protected: + BufferedSerial mod; + float acc[3], gyo[3], mag[3], att[3]; + void parseCmpt(int token, unsigned char* payloadBuf, int payloadLen); + void parseInput(const char* data, int len); +public: + JY901(PinName TX, PinName RX) : mod(TX, RX){} + ~JY901() {} + void receiveData(); + void getAcc(float &x, float &y, float &z){ + x = acc[0]; + y = acc[1]; + z = acc[2]; + } + void getGyo(float &x, float &y, float &z){ + x = gyo[0]; + y = gyo[1]; + z = gyo[2]; + } + void getMag(float &x, float &y, float &z){ + x = mag[0]; + y = mag[1]; + z = mag[2]; + } + void getAttitude(float &roll, float &pitch, float &yaw){ + roll = att[0]; + pitch = att[1]; + yaw = att[2]; + } +}; \ No newline at end of file
--- a/SoftPWM.lib Sun Dec 10 02:19:41 2017 +0000 +++ b/SoftPWM.lib Sun Dec 24 03:15:15 2017 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/komaida424/code/SoftPWM/#7918ce37626c +https://os.mbed.com/users/wanzq/code/SoftPWM/#6d557e90686e
--- 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; } - } - + } }