Qian Yuyang / Mbed 2 deprecated Fly_Test1

NRF+MPU6050

Dependencies:   mbed nRF24L01P

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?

UserRevisionLine numberNew 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 }