Diff: main.cpp

Revision:

3:46535ec6d8b1

Parent:

2:4704fdd9ef91

Child:

4:1b985e622d26

--- 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