Qian Yuyang
/
Fly_Test1
NRF+MPU6050
main.cpp@6:ad2f9458ab75, 2019-09-01 (annotated)
- Committer:
- AlexQian
- Date:
- Sun Sep 01 10:33:23 2019 +0000
- Revision:
- 6:ad2f9458ab75
- Parent:
- 5:6a93542b8922
test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
AlexQian | 6:ad2f9458ab75 | 1 | #include "mbed.h" |
AlexQian | 6:ad2f9458ab75 | 2 | #include "MPU6050.h" |
AlexQian | 6:ad2f9458ab75 | 3 | #include "nRF24L01P.h" |
AlexQian | 6:ad2f9458ab75 | 4 | #define NRF24_TRANSFER_SIZE 32 |
AlexQian | 6:ad2f9458ab75 | 5 | float Yaw,Pitch,Roll; |
AlexQian | 6:ad2f9458ab75 | 6 | float Yaw_t,Pitch_t,Roll_t,Power_t; |
AlexQian | 6:ad2f9458ab75 | 7 | float P,I,D; |
AlexQian | 6:ad2f9458ab75 | 8 | int Power[4]; |
AlexQian | 6:ad2f9458ab75 | 9 | int counts; |
AlexQian | 6:ad2f9458ab75 | 10 | char rxdata[NRF24_TRANSFER_SIZE]; |
AlexQian | 6:ad2f9458ab75 | 11 | |
AlexQian | 6:ad2f9458ab75 | 12 | DigitalOut myled1(PB_3); |
AlexQian | 6:ad2f9458ab75 | 13 | DigitalOut myled2(PB_4); |
AlexQian | 6:ad2f9458ab75 | 14 | mpu6050 mpu6050(PB_11,PB_10); |
AlexQian | 6:ad2f9458ab75 | 15 | Serial pc(PA_9, PA_10,115200); |
AlexQian | 6:ad2f9458ab75 | 16 | nRF24L01P nrf24_PB_15(PB_15,PB_14,PB_13,PA_4,PB_2,PB_12); |
AlexQian | 6:ad2f9458ab75 | 17 | PwmOut myServoPA_6(PA_6); |
AlexQian | 6:ad2f9458ab75 | 18 | PwmOut myServoPA_7(PA_7); |
AlexQian | 6:ad2f9458ab75 | 19 | PwmOut myServoPB_0(PB_0); |
AlexQian | 6:ad2f9458ab75 | 20 | PwmOut myServoPB_1(PB_1); |
AlexQian | 6:ad2f9458ab75 | 21 | |
AlexQian | 6:ad2f9458ab75 | 22 | void NRF_Init() |
AlexQian | 6:ad2f9458ab75 | 23 | { |
AlexQian | 6:ad2f9458ab75 | 24 | nrf24_PB_15.powerUp(); |
AlexQian | 6:ad2f9458ab75 | 25 | nrf24_PB_15.setTransferSize( NRF24_TRANSFER_SIZE ); |
AlexQian | 6:ad2f9458ab75 | 26 | nrf24_PB_15.setReceiveMode(); |
AlexQian | 6:ad2f9458ab75 | 27 | nrf24_PB_15.setRxAddress(12345ull); |
AlexQian | 6:ad2f9458ab75 | 28 | nrf24_PB_15.setTxAddress(12345ull); |
AlexQian | 6:ad2f9458ab75 | 29 | nrf24_PB_15.enable(); |
AlexQian | 6:ad2f9458ab75 | 30 | } |
AlexQian | 6:ad2f9458ab75 | 31 | void MPU_Init() |
AlexQian | 6:ad2f9458ab75 | 32 | { |
AlexQian | 6:ad2f9458ab75 | 33 | pc.printf("Initializing...\n"); |
AlexQian | 6:ad2f9458ab75 | 34 | while(mpu6050.Init()) //初始化 |
AlexQian | 6:ad2f9458ab75 | 35 | { |
AlexQian | 6:ad2f9458ab75 | 36 | counts+=1; |
AlexQian | 6:ad2f9458ab75 | 37 | wait(1); |
AlexQian | 6:ad2f9458ab75 | 38 | myled2=!myled2; |
AlexQian | 6:ad2f9458ab75 | 39 | if(counts>10) |
AlexQian | 6:ad2f9458ab75 | 40 | { |
AlexQian | 6:ad2f9458ab75 | 41 | pc.printf("Initialation failed\n"); // 初始化失败 |
AlexQian | 6:ad2f9458ab75 | 42 | break; |
AlexQian | 6:ad2f9458ab75 | 43 | } |
AlexQian | 6:ad2f9458ab75 | 44 | } |
AlexQian | 6:ad2f9458ab75 | 45 | pc.printf("Initialized\n"); //初始化完成 |
AlexQian | 6:ad2f9458ab75 | 46 | for(int i=0;i<20000;i++) //校零 |
AlexQian | 6:ad2f9458ab75 | 47 | mpu6050.receiveData(&Yaw,&Pitch,&Roll); //读取传感器姿态 |
AlexQian | 6:ad2f9458ab75 | 48 | } |
AlexQian | 6:ad2f9458ab75 | 49 | void motor(int pwm,int number) |
AlexQian | 6:ad2f9458ab75 | 50 | { |
AlexQian | 6:ad2f9458ab75 | 51 | switch(number) |
AlexQian | 6:ad2f9458ab75 | 52 | { |
AlexQian | 6:ad2f9458ab75 | 53 | case 0: |
AlexQian | 6:ad2f9458ab75 | 54 | myServoPA_7.period_ms(20); |
AlexQian | 6:ad2f9458ab75 | 55 | myServoPA_7.pulsewidth_ms(((pwm * 20) / 1000)); |
AlexQian | 6:ad2f9458ab75 | 56 | break; |
AlexQian | 6:ad2f9458ab75 | 57 | case 1: |
AlexQian | 6:ad2f9458ab75 | 58 | myServoPA_6.period_ms(20); |
AlexQian | 6:ad2f9458ab75 | 59 | myServoPA_6.pulsewidth_ms(((pwm * 20) / 1000)); |
AlexQian | 6:ad2f9458ab75 | 60 | break; |
AlexQian | 6:ad2f9458ab75 | 61 | case 2: |
AlexQian | 6:ad2f9458ab75 | 62 | myServoPB_0.period_ms(20); |
AlexQian | 6:ad2f9458ab75 | 63 | myServoPB_0.pulsewidth_ms(((pwm * 20) / 1000)); |
AlexQian | 6:ad2f9458ab75 | 64 | break; |
AlexQian | 6:ad2f9458ab75 | 65 | case 3: |
AlexQian | 6:ad2f9458ab75 | 66 | myServoPB_1.period_ms(20); |
AlexQian | 6:ad2f9458ab75 | 67 | myServoPB_1.pulsewidth_ms(((pwm * 20) / 1000)); |
AlexQian | 6:ad2f9458ab75 | 68 | break; |
AlexQian | 6:ad2f9458ab75 | 69 | default: |
AlexQian | 6:ad2f9458ab75 | 70 | break; |
AlexQian | 6:ad2f9458ab75 | 71 | } |
AlexQian | 6:ad2f9458ab75 | 72 | } |
AlexQian | 6:ad2f9458ab75 | 73 | void angle_control() |
AlexQian | 6:ad2f9458ab75 | 74 | { |
AlexQian | 6:ad2f9458ab75 | 75 | double ERoll,EPitch,EYaw,ERoll_last,EPitch_last,EYaw_last; |
AlexQian | 6:ad2f9458ab75 | 76 | double Interg_R,Interg_P,Interg_Y; |
AlexQian | 6:ad2f9458ab75 | 77 | double out_T,out_R,out_P,out_Y; |
AlexQian | 6:ad2f9458ab75 | 78 | ERoll=Roll-Roll_t; |
AlexQian | 6:ad2f9458ab75 | 79 | EPitch=Pitch-Pitch_t; |
AlexQian | 6:ad2f9458ab75 | 80 | EYaw=Yaw-Yaw_t; |
AlexQian | 6:ad2f9458ab75 | 81 | Interg_R+=ERoll; Interg_P+=EPitch; Interg_Y+=EYaw; |
AlexQian | 6:ad2f9458ab75 | 82 | // out_R=P*ERoll + I*Interg_R + D*(ERoll-ERoll_last); |
AlexQian | 6:ad2f9458ab75 | 83 | // out_P=P*EPitch + I*Interg_P + D*(EPitch-EPitch_last); |
AlexQian | 6:ad2f9458ab75 | 84 | // out_Y=P*EYaw + I*Interg_Y + D*(EYaw-EYaw_last); |
AlexQian | 6:ad2f9458ab75 | 85 | if(Power_t>200) |
AlexQian | 6:ad2f9458ab75 | 86 | { |
AlexQian | 6:ad2f9458ab75 | 87 | out_T=Power_t-50; |
AlexQian | 6:ad2f9458ab75 | 88 | Power[0]=out_T+out_R-out_P+out_Y; |
AlexQian | 6:ad2f9458ab75 | 89 | Power[1]=out_T-out_R-out_P-out_Y; |
AlexQian | 6:ad2f9458ab75 | 90 | Power[2]=out_T-out_R+out_P+out_Y; |
AlexQian | 6:ad2f9458ab75 | 91 | Power[3]=out_T+out_R+out_P-out_Y; |
AlexQian | 6:ad2f9458ab75 | 92 | } |
AlexQian | 6:ad2f9458ab75 | 93 | else |
AlexQian | 6:ad2f9458ab75 | 94 | Power[0]=Power[1]=Power[2]=Power[3]=0; |
AlexQian | 6:ad2f9458ab75 | 95 | for(int i=0;i<4;i++) |
AlexQian | 6:ad2f9458ab75 | 96 | motor(Power[i],i); |
AlexQian | 6:ad2f9458ab75 | 97 | } |
AlexQian | 6:ad2f9458ab75 | 98 | int main() |
AlexQian | 6:ad2f9458ab75 | 99 | { |
AlexQian | 6:ad2f9458ab75 | 100 | P=1;I=0;D=0; |
AlexQian | 6:ad2f9458ab75 | 101 | MPU_Init(); |
AlexQian | 6:ad2f9458ab75 | 102 | if(Yaw_t<10&&Yaw_t>-10) |
AlexQian | 6:ad2f9458ab75 | 103 | Yaw_t=0; |
AlexQian | 6:ad2f9458ab75 | 104 | while(1) |
AlexQian | 6:ad2f9458ab75 | 105 | { |
AlexQian | 6:ad2f9458ab75 | 106 | counts=counts+1; |
AlexQian | 6:ad2f9458ab75 | 107 | mpu6050.receiveData(&Yaw,&Pitch,&Roll); //读取传感器姿态 |
AlexQian | 6:ad2f9458ab75 | 108 | if (nrf24_PB_15.readable()) { |
AlexQian | 6:ad2f9458ab75 | 109 | nrf24_PB_15.read( NRF24L01P_PIPE_P0, rxdata, NRF24_TRANSFER_SIZE); |
AlexQian | 6:ad2f9458ab75 | 110 | if (rxdata[0] == 36 && rxdata[1] == 77) { |
AlexQian | 6:ad2f9458ab75 | 111 | Roll_t = rxdata[6] * 256 + rxdata[5]; |
AlexQian | 6:ad2f9458ab75 | 112 | Pitch_t = rxdata[8] * 256 + rxdata[7]; |
AlexQian | 6:ad2f9458ab75 | 113 | Yaw_t = rxdata[10] * 256 + rxdata[9]; |
AlexQian | 6:ad2f9458ab75 | 114 | Power_t = rxdata[12] * 256 + rxdata[11]; |
AlexQian | 6:ad2f9458ab75 | 115 | // Roll_Config = rxdata[14] * 256 + rxdata[13]; |
AlexQian | 6:ad2f9458ab75 | 116 | // Pitch_config = rxdata[16] * 256 + rxdata[15]; |
AlexQian | 6:ad2f9458ab75 | 117 | // Yaw_config = rxdata[18] * 256 + rxdata[17]; |
AlexQian | 6:ad2f9458ab75 | 118 | Roll_t =(Roll_t-1500)*45/1000; |
AlexQian | 6:ad2f9458ab75 | 119 | Pitch_t =(Pitch_t-1500)*45/1000 ; |
AlexQian | 6:ad2f9458ab75 | 120 | Yaw_t =(Yaw_t-1500)*45/1000 ; |
AlexQian | 6:ad2f9458ab75 | 121 | } |
AlexQian | 6:ad2f9458ab75 | 122 | } |
AlexQian | 6:ad2f9458ab75 | 123 | // pc.printf("Yaw, Pitch, Roll: %.2f, %.2f, %.2f\n", Yaw, Pitch, Roll); |
AlexQian | 6:ad2f9458ab75 | 124 | angle_control(); |
AlexQian | 6:ad2f9458ab75 | 125 | myled1=counts%2; |
AlexQian | 6:ad2f9458ab75 | 126 | wait(0.01); |
AlexQian | 6:ad2f9458ab75 | 127 | } |
AlexQian | 6:ad2f9458ab75 | 128 | } |