aurora moon / Mbed 2 deprecated withbufferserial_nrftst

四轴

Dependencies:   BufferedSerial SoftPWM mbed send

Fork of send_copy by aurora moon

main.cpp@5:683542d82dd7, 2017-12-10 (annotated)

Committer:
accelerator225
Date:
Sun Dec 10 02:19:41 2017 +0000
Revision:
5:683542d82dd7
Parent:
4:876bfa91934c 
??

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"
accelerator225 5:683542d82dd7 4 #define TRANSFER_SIZE 12
accelerator225 4:876bfa91934c 5 #include"JY901.h"
accelerator225 4:876bfa91934c 6 #define pwm_period 1
accelerator225 5:683542d82dd7 7 #define maxroll 15
accelerator225 5:683542d82dd7 8 #define maxpitch 15
accelerator225 5:683542d82dd7 9 #define maxyaw 15
accelerator225 5:683542d82dd7 10 #define maxAz 0.98
Owen 0:a51a6e7da590 11
accelerator225 5:683542d82dd7 12 char flag=0;//PID更新标志
accelerator225 5:683542d82dd7 13 SoftPWM PWM1(PA_2);
accelerator225 5:683542d82dd7 14 PwmOut PWM2(PA_3),PWM3(PA_6),PWM4(PA_7),PWM5(PB_0),PWM6(PB_1);//六路PWM输出接口（PA_2mbed不支持PWM输出 只能采用软件pwm）
accelerator225 5:683542d82dd7 15 float target_roll[3]={0,0,0},p_z=0.5;//目标
accelerator225 5:683542d82dd7 16 float err_old[3]={0,0,0};//记录前一个的误差值
accelerator225 5:683542d82dd7 17 float kp1=0.0015,ki1=5e-5,kd1=4e-2,
accelerator225 5:683542d82dd7 18 kp2=0,ki2=0,kd2=0,
accelerator225 5:683542d82dd7 19 kp0=0.001,ki0=5e-5,kd0=6e-2;//pid参数设置
accelerator225 5:683542d82dd7 20 float I[3]={0,0,0};//积分值
accelerator225 5:683542d82dd7 21 JY901 _JY901(PA_9,PA_10);//JY901 接口 （波特率为115200）
accelerator225 5:683542d82dd7 22 nRF24L01P my_nrf24l01p(PB_15, PB_14, PB_13, PC_1, PC_2, PC_0);//nrf模块
accelerator225 5:683542d82dd7 23 char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE];
Owen 0:a51a6e7da590 24
accelerator225 5:683542d82dd7 25 void flag_change(){flag=1;}
accelerator225 5:683542d82dd7 26 void pid_control(){
accelerator225 4:876bfa91934c 27 _JY901.receiveData();
accelerator225 5:683542d82dd7 28 float roll[3],P1,P2,P3,P4,P5,P6;
accelerator225 5:683542d82dd7 29 _JY901.getAttitude(roll[0], roll[1], roll[2]);
accelerator225 5:683542d82dd7 30 if(roll[0]>180) roll[0]-=360;
accelerator225 5:683542d82dd7 31 if(roll[1]>180) roll[1]-=360;
accelerator225 5:683542d82dd7 32 if(roll[2]>180) roll[2]-=360;//更新数据
accelerator225 5:683542d82dd7 33
accelerator225 5:683542d82dd7 34 P1=p_z;P2=p_z;P3=p_z;P4=p_z;P5=p_z;P6=p_z;
accelerator225 5:683542d82dd7 35 float err[3];
accelerator225 5:683542d82dd7 36 float pi1,pi2,pi0;
accelerator225 5:683542d82dd7 37 err[0]=target_roll[0]-roll[0];
accelerator225 5:683542d82dd7 38 err[1]=target_roll[1]-roll[1];
accelerator225 5:683542d82dd7 39 err[2]=target_roll[2]-roll[2];
accelerator225 5:683542d82dd7 40
accelerator225 5:683542d82dd7 41 I[0]+=err[0];
accelerator225 5:683542d82dd7 42 I[1]+=err[1];
accelerator225 5:683542d82dd7 43 I[2]+=err[2];
accelerator225 5:683542d82dd7 44
accelerator225 5:683542d82dd7 45 pi0=err[0]*kp0+ki0*I[0]+kd0*(err[0]-err_old[0]);
accelerator225 5:683542d82dd7 46 pi1=err[1]*kp1+ki1*I[1]+kd1*(err[1]-err_old[1]);
accelerator225 5:683542d82dd7 47 pi2=err[2]*kp2+ki2*I[2]+kd2*(err[2]-err_old[2]);//计算pid
accelerator225 5:683542d82dd7 48
accelerator225 5:683542d82dd7 49 err_old[1]=err[1];
accelerator225 5:683542d82dd7 50 err_old[2]=err[2];
accelerator225 5:683542d82dd7 51 err_old[0]=err[0];
accelerator225 5:683542d82dd7 52
accelerator225 5:683542d82dd7 53 if(pi1>0.25) pi1=0.25;//pid限幅
accelerator225 5:683542d82dd7 54 if(pi1<-0.25) pi1=-0.25;
accelerator225 5:683542d82dd7 55 if(pi0>0.25) pi0=0.25;
accelerator225 5:683542d82dd7 56 if(pi0<-0.25) pi0=-0.25;
accelerator225 5:683542d82dd7 57 if(pi2>0.25) pi2=0.25;
accelerator225 5:683542d82dd7 58 if(pi2<-0.25) pi2=-0.25;
accelerator225 5:683542d82dd7 59
accelerator225 5:683542d82dd7 60 P3+=(-pi1-pi2);
accelerator225 5:683542d82dd7 61 P6+=(pi1+pi2);//计算电机pwm占空比
accelerator225 5:683542d82dd7 62 P1+=(pi0-0.5*pi2);
accelerator225 5:683542d82dd7 63 P2+=(pi0+0.5*pi2);
accelerator225 5:683542d82dd7 64 P4+=(-pi0+0.5*pi2);
accelerator225 5:683542d82dd7 65 P5+=(-pi0-0.5*pi2);
accelerator225 5:683542d82dd7 66
accelerator225 5:683542d82dd7 67 PWM3=P3;PWM6=P6;
accelerator225 5:683542d82dd7 68 // if(P1<=0.001) PWM1=0.001;else PWM1=P1;
accelerator225 5:683542d82dd7 69 //PWM2=P2;PWM4=P4;PWM5=P5;//更新pwm占空比
accelerator225 4:876bfa91934c 70 }
accelerator225 5:683542d82dd7 71 void init()
accelerator225 4:876bfa91934c 72 {
accelerator225 5:683542d82dd7 73 my_nrf24l01p.powerUp();
accelerator225 5:683542d82dd7 74 my_nrf24l01p.setTransferSize(TRANSFER_SIZE);
accelerator225 5:683542d82dd7 75 my_nrf24l01p.setReceiveMode();
accelerator225 5:683542d82dd7 76 my_nrf24l01p.enable();
accelerator225 5:683542d82dd7 77 PWM1=0.0;PWM2=0.0;PWM3=0.0;PWM4=0.0;PWM5=0.0;PWM6=0.0;
accelerator225 5:683542d82dd7 78 }
accelerator225 5:683542d82dd7 79 int updata_target()
accelerator225 5:683542d82dd7 80 {
accelerator225 5:683542d82dd7 81 switch (rxData[0]){
accelerator225 5:683542d82dd7 82 case 0x50:
accelerator225 5:683542d82dd7 83 return 1;
accelerator225 5:683542d82dd7 84 // break;
accelerator225 5:683542d82dd7 85 case 0x51:
accelerator225 5:683542d82dd7 86 my_nrf24l01p.read( NRF24L01P_PIPE_P0, rxData, sizeof( rxData ) );
accelerator225 5:683542d82dd7 87 target_roll[0] = (float)(((int)rxData[1]*256+(int)rxData[2])-32768)*(float)maxroll/65536.0f;
accelerator225 5:683542d82dd7 88 // Az =(float) ((int)rxData[3]*256+(int)rxData[4]-32768)*(float)maxAz/65536.0f;
accelerator225 5:683542d82dd7 89 target_roll[1] = (float)(((int)rxData[5]*256+(int)rxData[6])-32768)*(float)maxpitch/65536.0f;
accelerator225 5:683542d82dd7 90 target_roll[2] = (float)(((int)rxData[7]*256+(int)rxData[8])-32768)*(float)maxyaw/65536.0f;
accelerator225 5:683542d82dd7 91 return -1;
accelerator225 5:683542d82dd7 92 // break;
accelerator225 5:683542d82dd7 93 case 0x52:
accelerator225 5:683542d82dd7 94 return 2;
accelerator225 5:683542d82dd7 95 // break;
accelerator225 5:683542d82dd7 96 }
accelerator225 5:683542d82dd7 97 return 0;
accelerator225 5:683542d82dd7 98 }
accelerator225 5:683542d82dd7 99 int state=0;
accelerator225 5:683542d82dd7 100 int main() {
accelerator225 5:683542d82dd7 101 init();
accelerator225 5:683542d82dd7 102 Ticker time;
accelerator225 5:683542d82dd7 103 wait(1);
accelerator225 5:683542d82dd7 104 // time.attach(&pid_control,0.01);
accelerator225 5:683542d82dd7 105 while(1){
accelerator225 5:683542d82dd7 106 //if(flag){flag=0;}
accelerator225 5:683542d82dd7 107
accelerator225 5:683542d82dd7 108 if(my_nrf24l01p.readable()) my_nrf24l01p.read(NRF24L01P_PIPE_P0,rxData,sizeof(rxData));
accelerator225 5:683542d82dd7 109 switch(state)
accelerator225 5:683542d82dd7 110 {
accelerator225 4:876bfa91934c 111 case 0:
accelerator225 5:683542d82dd7 112 if (updata_target()==1) {state=1; time.attach(&pid_control,0.01); }
accelerator225 4:876bfa91934c 113 break;
accelerator225 5:683542d82dd7 114
accelerator225 4:876bfa91934c 115 case 1:
accelerator225 5:683542d82dd7 116 if (updata_target()==2) state=2;
accelerator225 4:876bfa91934c 117 break;
accelerator225 5:683542d82dd7 118
accelerator225 4:876bfa91934c 119 case 2:
accelerator225 5:683542d82dd7 120
accelerator225 4:876bfa91934c 121 break;
accelerator225 5:683542d82dd7 122 }
accelerator225 4:876bfa91934c 123 }
accelerator225 4:876bfa91934c 124
accelerator225 4:876bfa91934c 125 }
Owen 0:a51a6e7da590 126