Diff: main.cpp

Revision:

1:f9658c7309ef

Parent:

0:badebd32bd8b

Child:

2:3a7eb05dbc72

diff -r badebd32bd8b -r f9658c7309ef main.cpp
--- a/main.cpp	Mon Apr 09 14:34:45 2018 +0000
+++ b/main.cpp	Fri Apr 20 08:27:50 2018 +0000
@@ -1,12 +1,76 @@
 #include "mbed.h"
 #include "mpu6050.h" 
+#define PWM_PERIOD 1
+#define RATE 0.01
 
 
-DigitalOut myled(LED1);
-Serial pc(SERIAL_TX, SERIAL_RX);
+float NTMAX=100.0;
+struct PID
+{
+    float Kp;
+    float Ki;
+    float Kd;
+    float error;
+    float dif_error;
+    float last_error;
+    float int_error;
+};
+//0.00005
+
+//0.00015
+PID PID_Para={0.0,0.0,0.0005};
+Ticker toggle_led_ticker;   //声明一个 Ticker 对象
+DigitalOut myled(PC_13);
+//Serial pc(PA_9, PA_10);
+float pitch,roll,yaw,co;       //欧拉角
+PwmOut mypwm1(PA_7);
+PwmOut mypwm2(PA_6);
+PwmOut mypwm3(PB_0);
+PwmOut mypwm4(PB_1);
+float calculate(float input,float target);
+void make() {        //中断子程序,在平衡车和无人机的项目中,控制算法就写在中断子程序中:获取陀螺仪数据,根据陀螺仪数据通过PID算法得到电机的控制量(占空比和方向),在输出到单片机管脚
+         co = calculate(roll, 0.0);
+           // pc.printf("pitch: %.2f roll: %.2f yaw: %.2f co: %.2f\r\n", pitch,roll,yaw,co);
+    // pc.printf("roll: %.2f co: %.2f\r\n",roll,co);
+     if (co >=0)
+         {  
+        mypwm2.pulsewidth(co);
+        mypwm1.pulsewidth(0);
+        mypwm4.pulsewidth(co);
+        mypwm3.pulsewidth(0);}
+        else
+        {
+        mypwm2.pulsewidth(0);
+        mypwm1.pulsewidth(-co);
+        mypwm4.pulsewidth(0);
+        mypwm3.pulsewidth(-co);}
+}
+
+
+float calculate(float input,float target)
+{
+    PID_Para.error=target-input;
+    PID_Para.dif_error=PID_Para.error-PID_Para.last_error;
+    PID_Para.last_error=PID_Para.error;
+
+    PID_Para.int_error+=PID_Para.error;
+    if(PID_Para.int_error >= NTMAX)
+        PID_Para.int_error = NTMAX;
+    if(PID_Para.int_error <= -NTMAX)
+        PID_Para.int_error = -NTMAX;
+
+    float output;
+    output=PID_Para.Kp*PID_Para.error+PID_Para.Ki*PID_Para.int_error-PID_Para.Kd*PID_Para.dif_error;
+    return output;
+}
 
 int main() {
-    float pitch,roll,yaw;       //欧拉角
+    
+    mypwm1.period_ms(PWM_PERIOD);
+    mypwm2.period_ms(PWM_PERIOD);
+    mypwm3.period_ms(PWM_PERIOD);
+    mypwm4.period_ms(PWM_PERIOD);
+    
 
     MPU_Init();                 //初始化MPU6050
     myled = 0;
@@ -15,11 +79,10 @@
         wait(0.2);
         myled = !myled;
     }    
+  
+    toggle_led_ticker.attach(&make, RATE);
     while(1)
     {
-        if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)//获取欧拉角
-        { 
-            pc.printf("pitch: %.2f roll: %.2f yaw: %.2f\r\n", pitch,roll,yaw);
-        }
+       mpu_dmp_get_data(&pitch,&roll,&yaw);
     }   
 }