aurora moon / Mbed 2 deprecated withbufferserial_nrftst

四轴

Dependencies:   BufferedSerial SoftPWM mbed send

Fork of send_copy by aurora moon

Files at this revision

API Documentation at this revision

Revision 5:683542d82dd7, committed 2017-12-10

Comitter:
accelerator225
Date:
Sun Dec 10 02:19:41 2017 +0000
Parent:
4:876bfa91934c
Commit message:
??

Changed in this revision

BufferedSerial.lib Show annotated file Show diff for this revision Revisions of this file
JY901.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
diff -r 876bfa91934c -r 683542d82dd7 BufferedSerial.lib
--- a/BufferedSerial.lib	Wed Nov 08 06:24:17 2017 +0000
+++ b/BufferedSerial.lib	Sun Dec 10 02:19:41 2017 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/sam_grove/code/BufferedSerial/#a0d37088b405
+https://os.mbed.com/users/accelerator225/code/BufferedSerial/#19fd7170f21e

diff -r 876bfa91934c -r 683542d82dd7 JY901.h
--- a/JY901.h	Wed Nov 08 06:24:17 2017 +0000
+++ b/JY901.h	Sun Dec 10 02:19:41 2017 +0000
@@ -1,16 +1,16 @@
 #include "mbed.h"
-#include "BufferedSerial.h"
+#include "BufferedSerial.h"//波特率在这个头文件中改
 class JY901
 {
     int state, token, payloadLen, recvLen;
     unsigned char payloadBuf[16];
 protected:
-    BufferedSerial mod;
+        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, 32){}
+    JY901(PinName TX, PinName RX) : mod(TX, RX){}
     ~JY901() {}
     void receiveData();
     void getAcc(float &x, float &y, float &z){

diff -r 876bfa91934c -r 683542d82dd7 main.cpp
--- 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");
- 
-    
+        }
     } 
        
 }