fly_v1
Dependencies: BufferedSerial SoftPWM mbed send
Fork of withbufferserial_nrftst by
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
- Child:
- 6:6e0f02edb404
??
Who changed what in which revision?
User | Revision | Line number | New 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 |