Diff: main.cpp

Revision:

5:683542d82dd7

Parent:

4:876bfa91934c

--- a/main.cpp	Wed Nov 08 06:24:17 2017 +0000
+++ b/main.cpp	Sun Dec 10 02:19:41 2017 +0000
@@ -1,99 +1,125 @@
 #include "mbed.h"
 #include "nRF24L01P.h"
 #include "SoftPWM.h"
-#define JY901transfersize 11
-#define TRANSFER_SIZE   11
+#define TRANSFER_SIZE   12
 #include"JY901.h"
 #define pwm_period 1
-int flagg = 0;
-
-char targetdata[3];
-char receivedata[4];
-float change[4];
-int receivedatacnt = 0;
-SoftPWM PWM1(PA_2),PWM2(PA_3),PWM3(PA_6),PWM4(PA_7),PWM5(PB_0),PWM6(PB_1);
+#define maxroll 15
+#define maxpitch 15
+#define maxyaw 15
+#define maxAz 0.98
 
-int rollx=0,rolly=0,rollz=0,drx,dry,drz,ax,ay,az;
-float kp,ki,kd;
-void pwm_init()
-{
-    PWM1.period_ms(pwm_period);
-    PWM2.period_ms(pwm_period);
-    PWM3.period_ms(pwm_period);
-    PWM4.period_ms(pwm_period);
-    PWM5.period_ms(pwm_period);
-    PWM6.period_ms(pwm_period);
-    PWM1=0.0;PWM2=0.0;PWM3=0.0;PWM4=0.0;PWM5=0.0;PWM6=0.0;
-}
+char flag=0;//PID更新标志
+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;//目标
+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];
 
-JY901 _JY901(PA_9,PA_10);
-//nRF24L01P my_nrf24l01p(PB_15, PB_14, PB_13, PC_1, PC_2, PC_0);
-char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE];
-    int JY901cnt = 0;
-    int txDataCnt = 0;
-    int rxDataCnt = 0;
-void transferdata(){
+void flag_change(){flag=1;}
+void pid_control(){
     _JY901.receiveData();
-    float acc[3];
-            _JY901.getAttitude(acc[0], acc[1], acc[2]);
-           PWM1=0.2;//0.13+0.13*(float)rollx/32767.0;
-        PWM2=0.4;//0.13-0.13*(float)rolly/32767.0;
-        PWM3=0.1-0.1*acc[0]/180;
+    float roll[3],P1,P2,P3,P4,P5,P6;
+    _JY901.getAttitude(roll[0], roll[1], roll[2]);
+    if(roll[0]>180) roll[0]-=360;
+    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;
+    float err[3];
+    float pi1,pi2,pi0;
+    err[0]=target_roll[0]-roll[0];  
+    err[1]=target_roll[1]-roll[1];
+    err[2]=target_roll[2]-roll[2];
+    
+    I[0]+=err[0];
+    I[1]+=err[1]; 
+    I[2]+=err[2];
+
+    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
+    
+    err_old[1]=err[1];
+    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;
+     
+    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);
+    
+    PWM3=P3;PWM6=P6;
+   // if(P1<=0.001) PWM1=0.001;else PWM1=P1;
+   //PWM2=P2;PWM4=P4;PWM5=P5;//更新pwm占空比
 }
-
-int PIDcontrol(char *targetdata,char *data,float *change){
-    return 1;
-}
-
-void update_roll()
+void init()
 {
-   // char *s;
-    /*char data[JY901transfersize];
-    char flag =0,flag2 = 0;
-    int q=1;
-    while(q < 1000){
-        q++;
-        switch(flag){
+    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()
+{
+   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;
+}
+int state=0;
+int main() { 
+    init();
+    Ticker time;
+    wait(1);
+  // time.attach(&pid_control,0.01);
+    while(1){
+    //if(flag){flag=0;}
+
+        if(my_nrf24l01p.readable()) my_nrf24l01p.read(NRF24L01P_PIPE_P0,rxData,sizeof(rxData));
+        switch(state)
+        {
             case 0:
-                flag2=JY901.getc();
-                if(flag2==0x55) flag++;
+            if (updata_target()==1) {state=1; time.attach(&pid_control,0.01); }
             break;
+            
             case 1:
-                flag2=JY901.getc();
-                if(flag2 == 0x53){
-                    flag++;
-                    data[0] = 0x55;
-                    data[1] = flag2;
-                    }
-                else flag--;
+            if (updata_target()==2) state=2;
             break;
+            
             case 2:
-                for(int i = 2;i != JY901transfersize;i++)
-                    data[i] = JY901.getc();
-                q = 1001;
+            
             break;
-        }        
-    }*/
-
-   // rollx+=drx;
-   // rolly+=dry;
-    //rollz+=drz;
-   // sprintf(s,"%f",rollx*180/32767);
-   // my_nrf24l01p.write( NRF24L01P_PIPE_P0,s,11);
-}
-int main() { 
-  //  my_nrf24l01p.powerUp();  
-   // my_nrf24l01p.setTransferSize( 11);
-   // my_nrf24l01p.setReceiveMode();
-  //  my_nrf24l01p.enable();  
-
-    Ticker time;
-    time.attach(&transferdata,0.1); 
-    while(1){
-   
-       // pc.printf("succes");
- 
-    
+        }
     } 
        
 }