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;
         }
-    } 
-       
+     }  
 }