Diff: main.cpp

Revision:

6:ad2f9458ab75

Parent:

5:6a93542b8922

--- a/main.cpp	Mon Apr 15 05:16:57 2019 +0000
+++ b/main.cpp	Sun Sep 01 10:33:23 2019 +0000
@@ -0,0 +1,128 @@
+#include "mbed.h"
+#include "MPU6050.h"
+#include "nRF24L01P.h"
+#define NRF24_TRANSFER_SIZE 32
+float Yaw,Pitch,Roll;
+float Yaw_t,Pitch_t,Roll_t,Power_t;
+float P,I,D;
+int Power[4];
+int counts;
+char rxdata[NRF24_TRANSFER_SIZE];
+
+DigitalOut myled1(PB_3);
+DigitalOut myled2(PB_4);
+mpu6050 mpu6050(PB_11,PB_10);
+Serial pc(PA_9, PA_10,115200);
+nRF24L01P nrf24_PB_15(PB_15,PB_14,PB_13,PA_4,PB_2,PB_12);
+PwmOut myServoPA_6(PA_6);
+PwmOut myServoPA_7(PA_7);
+PwmOut myServoPB_0(PB_0);
+PwmOut myServoPB_1(PB_1);
+
+void NRF_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();
+}
+void MPU_Init()
+{
+    pc.printf("Initializing...\n");
+    while(mpu6050.Init())  //初始化
+    {
+        counts+=1;
+        wait(1);
+        myled2=!myled2;
+        if(counts>10)
+        {
+            pc.printf("Initialation failed\n"); // 初始化失败
+            break;
+        }   
+    }
+    pc.printf("Initialized\n"); //初始化完成
+    for(int i=0;i<20000;i++) //校零
+    mpu6050.receiveData(&Yaw,&Pitch,&Roll);  //读取传感器姿态
+}
+void motor(int pwm,int number)
+{
+    switch(number)
+    {
+    case 0:
+        myServoPA_7.period_ms(20);
+        myServoPA_7.pulsewidth_ms(((pwm * 20) / 1000));
+        break;
+    case 1:
+        myServoPA_6.period_ms(20);
+        myServoPA_6.pulsewidth_ms(((pwm * 20) / 1000));
+        break;
+    case 2:
+        myServoPB_0.period_ms(20);
+        myServoPB_0.pulsewidth_ms(((pwm * 20) / 1000));
+        break;
+    case 3:
+        myServoPB_1.period_ms(20);
+        myServoPB_1.pulsewidth_ms(((pwm * 20) / 1000));
+        break;
+    default:
+        break;
+    }
+}
+void angle_control()
+{
+    double ERoll,EPitch,EYaw,ERoll_last,EPitch_last,EYaw_last;
+    double Interg_R,Interg_P,Interg_Y;
+    double out_T,out_R,out_P,out_Y;
+    ERoll=Roll-Roll_t;
+    EPitch=Pitch-Pitch_t;
+    EYaw=Yaw-Yaw_t;
+    Interg_R+=ERoll; Interg_P+=EPitch; Interg_Y+=EYaw;
+//    out_R=P*ERoll + I*Interg_R + D*(ERoll-ERoll_last);
+//    out_P=P*EPitch + I*Interg_P + D*(EPitch-EPitch_last);
+//    out_Y=P*EYaw + I*Interg_Y + D*(EYaw-EYaw_last);
+    if(Power_t>200)
+   { 
+        out_T=Power_t-50;
+        Power[0]=out_T+out_R-out_P+out_Y;
+        Power[1]=out_T-out_R-out_P-out_Y;
+        Power[2]=out_T-out_R+out_P+out_Y;
+        Power[3]=out_T+out_R+out_P-out_Y;
+    }
+    else
+        Power[0]=Power[1]=Power[2]=Power[3]=0;
+    for(int i=0;i<4;i++)
+        motor(Power[i],i);    
+}
+int main()
+{
+    P=1;I=0;D=0;
+    MPU_Init();
+    if(Yaw_t<10&&Yaw_t>-10)
+        Yaw_t=0;
+    while(1) 
+    {
+        counts=counts+1;
+        mpu6050.receiveData(&Yaw,&Pitch,&Roll);  //读取传感器姿态 
+        if (nrf24_PB_15.readable()) {
+            nrf24_PB_15.read( NRF24L01P_PIPE_P0, rxdata, NRF24_TRANSFER_SIZE);
+            if (rxdata[0] == 36 && rxdata[1] == 77) {
+                Roll_t = rxdata[6] * 256 + rxdata[5];
+                Pitch_t = rxdata[8] * 256 + rxdata[7];
+                Yaw_t = rxdata[10] * 256 + rxdata[9];
+                Power_t = rxdata[12] * 256 + rxdata[11];
+        //      Roll_Config = rxdata[14] * 256 + rxdata[13];
+        //      Pitch_config = rxdata[16] * 256 + rxdata[15];
+        //      Yaw_config = rxdata[18] * 256 + rxdata[17];
+                Roll_t =(Roll_t-1500)*45/1000;
+                Pitch_t =(Pitch_t-1500)*45/1000 ;
+                Yaw_t =(Yaw_t-1500)*45/1000 ;
+            }
+        }
+//      pc.printf("Yaw, Pitch, Roll: %.2f, %.2f, %.2f\n", Yaw, Pitch, Roll);
+        angle_control();
+        myled1=counts%2;
+        wait(0.01);
+    }
+ }
\ No newline at end of file