论坛 › 你问我答 › flymaple PID 的 输出 问题

怪怪金小鱼 发表于 2012-10-11 20:48:49

flymaple PID 的 输出 问题

本帖最后由 Ricky 于 2012-12-26 23:31 编辑

#include <Wire.h>
#define Gry_offset 15   // 陀螺仪偏移量
#define Gyr_Gain 0.07   // 满量程2000dps时灵敏度(dps/digital)
#define pi 3.14159

float Com_angle;
float Y1;
float Com2_angle;
float Klm_angle;

#define Q_angle 0.01      // 角度数据置信度
#define Q_omega 0.0003    // 角速度数据置信度
#define R_angle 0.01      // 方差噪声
float bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float angleG;
long timer = 0;// 采样时间

void lvbo(void)
{
/********** 互补滤波器参数 *********/
unsigned long preTime = 0; // 采样时间
float f_angle = 0.0;       // 滤波处理后的角度值

/*********** PID控制器参数 *********/
unsigned long lastTime;         // 前次时间
float ITerm, lastInput;    // 积分项、前次输入
float Output = 0.0;      // PID输出值




unsigned long now = millis();                           // 当前时间(ms)
    float dt = (now - preTime) / 1000.0;                  // 微分时间(s)
    preTime = now;                                          // 记录本次时间(ms)

/********** 读取姿态传感器 *********/
    int16 acc;
    getAccelerometerData(acc);
    int16 gyro;
    getGyroscopeData(gyro);    //读取陀螺仪


    float Y_Acc =acc;      //(Acc, 1);                           // 获取向前的加速度(digite)
    float Z_Acc =acc;       // (Acc, 2);                           // 获取向下的加速度(digite)
    float angleA = atan(Y_Acc / Z_Acc) * 180 / pi;          // 根据加速度分量得到的角度(degree)
    float omega =Gyr_Gain * (gyro+Gry_offset); // 当前角速度(degree/s)

/*********** 一阶互补滤波 **********/
    float K = 0.8;                                          // 取值权重
    float A = K / (K + dt);                                 // 加权系数
    f_angle = A * (f_angle + omega * dt) + (1-A) * angleA;// 互补滤波算法
/************ PID控制器 ***********/
    now = millis();                                       // 当前时间(ms)
    float TimeCh = (now - lastTime) / 1000.0;               // 采样时间(s)
    float Kp = 10.0, Ki = 0.0, Kd = 0.0;                  // 比例系数、积分系数、微分系数
    float SampleTime = 0.1;                                 // 采样时间(s)
    float Setpoint = -3.8;                                  // 设定目标值(degree)
<font color="#ff0000">    float outMin = -80.0, outMax = +80.0;                   // 输出上限、输出下限</font>
<font color="#ff0000">    if(TimeCh >= SampleTime) {                              // 到达预定采样时间时</font>
<font color="#ff0000">      float Input = f_angle;                              // 输入赋值</font>
<font color="#ff0000">      float error = Setpoint - Input;                     // 偏差值</font>
<font color="#ff0000">      ITerm+= (Ki * error * TimeCh);                      // 计算积分项</font>
<font color="#ff0000">      ITerm = constrain(ITerm, outMin, outMax);         // 限定值域</font>
<font color="#ff0000">      float DTerm = Kd * (Input - lastInput) / TimeCh;    // 计算微分项</font>
<font color="#ff0000">      Output = Kp * error + ITerm - DTerm;                // 计算输出值</font>
<font color="#ff0000">      Output = constrain(Output, outMin, outMax);         // 限定值域</font>
<font style="background-color:yellow">       // servoL.write(Output + servoL_offset);               // 控制左驱</font>
<font style="background-color:yellow">       // servoR.write(Output + servoR_offset);               // 控制右驱</font>
      lastInput = Input;                                  // 记录输入值
      lastTime = now;                                     // 记录本次时间
    }
/************ 参数上传 ***********/
    SerialUSB.print(now);          // 计算时间
    SerialUSB.print(", ");
    SerialUSB.print(f_angle, 6);   // 偏离角度
    SerialUSB.print(", ");
    SerialUSB.print(Output, 6);    // PID输出值
    SerialUSB.print("; ");

    SerialUSB.println("");
// 控制微分时间
    delay(100);
}这是 来自极客工坊 的 一段 自平衡小车 的程序

怪怪金小鱼 发表于 2012-10-11 20:52:17

RE: flymaple PID 的 输出 问题

标记 部分 是 控制小车的 电机的PID 的输出 是怎么与 我控制 电调 相连的 我不知道 怎么写了还有这个 输出 一直 是 这样
一直是 80 ？？？为什么 ？

怪怪金小鱼 发表于 2012-10-12 10:53:07

求助呀 :'(

夏斌 发表于 2012-12-26 22:45:53

本帖最后由 夏斌 于 2012-12-26 22:48 编辑

你旋转一下板子

夏斌 发表于 2012-12-26 22:53:26

刚看了下时间，原来好久的事啦:(

怪怪金小鱼 发表于 2012-12-27 11:08:23

夏斌 发表于 2012-12-26 22:53 static/image/common/back.gif
刚看了下时间，原来好久的事啦

我 这个 都 放下 好久 了 一直也没进行算法那块 我是 小白编程也 不好。。。

夏斌 发表于 2012-12-27 12:13:42

怪怪金小鱼 发表于 2012-12-27 11:08 static/image/common/back.gif
我 这个 都 放下 好久 了 一直也没进行算法那块 我是 小白编程也 不好。。。
...

我刚拿到板子时间不长，还没来得及研究算法呢，不过是做了点类似膝跳反射的响应程序。

查看完整版本: flymaple PID 的 输出 问题