Revision 1:db5ac8bf9280, committed 2018-05-20

Comitter:

glintligo

Date:

Sun May 20 07:02:38 2018 +0000

Parent:

0:badebd32bd8b

Commit message:

????

Changed in this revision

--- a/main.cpp	Mon Apr 09 14:34:45 2018 +0000
+++ b/main.cpp	Sun May 20 07:02:38 2018 +0000
@@ -1,25 +1,125 @@
 #include "mbed.h"
 #include "mpu6050.h" 
+#define PWM_PERIOD 1    //pwm波的周期  单位ms
+#define RATE 0.01       //定时中断的周期，单位s
+
+
+struct PID
+{
+    float Kp;
+    float Ki;
+    float Kd;
+    float error;
+    float dif_error;
+    float last_error;
+    float int_error;
+};
+
+                           
+float NTMAX=100.0;                          //最大容错
+float t =0.0;                               //pid控制的目标
+PID PID_Para={0.00008,0.0,-0.00057};        //pid参数,kp,kp,kd
+float pitch,roll,yaw;                       //欧拉角
+float co;                                   //pid输出结果，作为pwm波的输入
 
 
-DigitalOut myled(LED1);
-Serial pc(SERIAL_TX, SERIAL_RX);
+Ticker toggle_time_ticker;                  //时间中断，用来控制转速的变化
+DigitalOut myled(PC_13);                    //检测mpu是否正常初始化，正常初始化后不会闪烁
+Serial pc(PA_9, PA_10);                     //蓝牙串口
+PwmOut mypwm1(PA_7);                        //控制电机的pwm波
+PwmOut mypwm2(PA_6);                        //控制电机的pwm波
+PwmOut mypwm3(PB_0);                        //控制电机的pwm波
+PwmOut mypwm4(PB_1);                        //控制电机的pwm波
+
+/**
+ * [calculate 计算pid控制结果]
+ * @param  input  [pid的输入]
+ * @param  target [pid控制目标]
+ * @return        [pid的输出]
+ */
+float calculate(float input,float target);
+/**
+ * [make 时间中断函数]
+ */
+void make();
+
 
 int main() {
-    float pitch,roll,yaw;       //欧拉角
+    //设定pwm波的周期 为1ms
+    mypwm1.period_ms(PWM_PERIOD);
+    mypwm2.period_ms(PWM_PERIOD);
+    mypwm3.period_ms(PWM_PERIOD);
+    mypwm4.period_ms(PWM_PERIOD);
 
-    MPU_Init();                 //初始化MPU6050
+    //初始化MPU6050，正常初始化后不会闪烁
+    MPU_Init();                 
     myled = 0;
     while(mpu_dmp_init())
     {
         wait(0.2);
         myled = !myled;
     }    
+
+    //设置定时中断的周期为100hz，调用中断函数为void make()
+    toggle_time_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数据，roll为俯仰角
+        mpu_dmp_get_data(&pitch,&roll,&yaw);
+    }  
+
+}
+
+/**
+ * [make 时间中断函数]
+ */
+void make() {
+    //计算pid控制结果        
+    co = calculate(roll, t); 
+    //控制电机
+    if (co >=0)
+    {  
+       mypwm1.pulsewidth(co);
+       mypwm2.pulsewidth(0);
+       mypwm3.pulsewidth(co);
+       mypwm4.pulsewidth(0);
+    }
+    else
+    {
+       mypwm1.pulsewidth(0);
+       mypwm2.pulsewidth(-co);
+       mypwm3.pulsewidth(0);
+       mypwm4.pulsewidth(-co);
+    }
 }
+
+/**
+ * [calculate 计算pid控制结果]
+ * @param  input  [pid的输入]
+ * @param  target [pid控制目标]
+ * @return        [pid的输出]
+ */
+float calculate(float input,float target)
+{
+    //计算输入与目标的偏差
+    PID_Para.error=target-input;
+    //设置死位
+    if(PID_Para.error<1.0 && PID_Para.error >-1.0) PID_Para.error=0.0;
+    //计算微分
+    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;
+    //计算pid结果
+    output=PID_Para.Kp*PID_Para.error+PID_Para.Ki*PID_Para.int_error-PID_Para.Kd*PID_Para.dif_error;
+    return output;
+}