Qian Yuyang
/
Ex_4Rotor_template
NRF+MPU6050
Diff: main.cpp
- Revision:
- 3:46535ec6d8b1
- Parent:
- 2:4704fdd9ef91
- Child:
- 4:1b985e622d26
diff -r 4704fdd9ef91 -r 46535ec6d8b1 main.cpp --- a/main.cpp Sun Dec 24 03:12:05 2017 +0000 +++ b/main.cpp Mon Mar 18 03:39:29 2019 +0000 @@ -1,87 +1,64 @@ +#define HIGH 1 +#define LOW 0 #include "mbed.h" #include "nRF24L01P.h" -#define TRANSFER_SIZE 12 -#define Send_Repeat_Times 10//命令重发次数 -#define command_roll 0x51 -#define command_start 0x50 -#define command_end 0x52 -#define command_slow 0x53 - -int flag=0;//发送状态标志 0:发送姿态命令 1:发送启动命令 2:发送急停命令 3:发送缓降命令 -char txdata[TRANSFER_SIZE]; - -nRF24L01P my_nrf24l01p(PB_15, PB_14, PB_13, PB_6, PB_5, PB_7); // mosi, miso, sck, csn, ce, irq +#include "MPU6050.h" +#include <string> +typedef bool boolean; +typedef std::string String; -DigitalOut myled1(PA_6); -DigitalOut myled2(PA_7); +char rxdata[12] ; +float Yaw,Pitch,Roll,Power; +int Yaw_t; +int Pitch_t; +int Roll_t; +int Power_t; +long CycleCount; +long ReceiveCount; -//DigitalIn sz_n(PB_10),sz_p(PA_5),sx_p(PB_0),sx_n(PB_1),sy_p(PB_3),sy_n(PB_4); -AnalogIn L_X(PA_0),L_Y(PA_1),R_X(PA_2),R_Y(PA_3); -InterruptIn START(PB_4),END(PB_3),SLOW(PB_0); -Serial pc(PA_9,PA_10,9600); - -void start(){flag=1;} -void end(){flag=2;} -void slow(){flag = 3;} - +#define NRF24_TRANSFER_SIZE 12 +nRF24L01P nrf24_PB_15(PB_15,PB_14,PB_13,PB_6,PB_5,PB_7); +Serial Serial_1(PA_2,PA_3); +DigitalOut myDigitalOutPA_7(PA_7); +DigitalOut myDigitalOutPA_6(PA_6); +MPU6050 mpu6050(PB_9,PB_8); int main() { - for(int i=0;i!=TRANSFER_SIZE;i++) - txdata[i] = 0; - my_nrf24l01p.powerUp(); - my_nrf24l01p.setTransferSize( TRANSFER_SIZE ); - my_nrf24l01p.setReceiveMode(); - my_nrf24l01p.enable(); - - myled1 = 0; - myled2 = 0; - - START.mode(PullDown); - END.mode(PullDown); - SLOW.mode(PullDown); - START.rise(&start); - END.rise(&end); - SLOW.rise(&slow); - while (1) { - switch (flag) - { - case 0: - txdata[0]= command_roll; - short int data[4]; - data[0] = L_X.read_u16(); - data[1] = L_Y.read_u16(); - data[2] = R_X.read_u16(); - data[3] = R_Y.read_u16(); - txdata[2] = data[0]; - txdata[1] = data[0] >> 8; - txdata[4] = data[1]; - txdata[3] = data[1] >> 8; - txdata[6] = data[2]; - txdata[5] = data[2] >> 8; - txdata[8] = data[3]; - txdata[7] = data[3] >> 8; - txdata[9] = 0; - txdata[10] = 0; - txdata[11] = 0; - my_nrf24l01p.write( NRF24L01P_PIPE_P0, txdata, 12); - break; - case 1: - txdata[0] = command_start; - for(int i=0;i<Send_Repeat_Times;i++) {my_nrf24l01p.write( NRF24L01P_PIPE_P0, txdata, 12); wait(0.01);} - flag = 0; - break; - case 2: - txdata[0] = command_end; - for(int i=0;i<Send_Repeat_Times;i++) {my_nrf24l01p.write( NRF24L01P_PIPE_P0, txdata, 12); wait(0.01);} - flag = 0; - break; - case 3: - txdata[0] = command_slow; - for(int i=0;i<Send_Repeat_Times;i++) {my_nrf24l01p.write( NRF24L01P_PIPE_P0, txdata, 12); wait(0.01);} - flag = 0; - break; - } - wait(0.05);//防止过快发送造成命令无法被接收 - } +Serial_1.baud(9600); +Serial_1.printf("Init\n"); +mpu6050.Init(); + +nrf24_PB_15.powerUp(); +nrf24_PB_15.setTransferSize( NRF24_TRANSFER_SIZE ); +nrf24_PB_15.setReceiveMode(); +nrf24_PB_15.setRxAddress(12345ull); +nrf24_PB_15.setTxAddress(12345ull); +nrf24_PB_15.enable(); + + + +CycleCount = 0; +ReceiveCount = 0; +Serial_1.printf("Ready to Receive\n"); +while (true) { +CycleCount = CycleCount + 1; + +mpu6050.receiveData(&Yaw,&Pitch,&Roll); //读取传感器姿态 +Serial_1.printf("Sensor Data: Yaw, Pitch, Roll: %.2f %.2f %.2f\n\r", Yaw, Pitch, Roll); + +if (nrf24_PB_15.readable()) { +ReceiveCount = ReceiveCount + 1; +myDigitalOutPA_7.write((ReceiveCount % 2 == 0)); +nrf24_PB_15.read( NRF24L01P_PIPE_P0, rxdata, NRF24_TRANSFER_SIZE); +if (rxdata[0] == 82) { +Power_t = rxdata[1]; +Yaw_t = rxdata[2] - 45; +Pitch_t = rxdata[3] - 45; +Roll_t = rxdata[4] - 45; +Serial_1.printf("Target: Power=%d, Yaw=%d, Pitch=%d, Roll=%d \n",Power_t,Yaw_t,Pitch_t,Roll_t); +} +} +myDigitalOutPA_6.write((CycleCount % 2 == 0)); +wait(0.01); } - +} \ No newline at end of file