flymaple PID 的 输出 问题

#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[3];
    getAccelerometerData(acc); 
    int16 gyro[4];
    getGyroscopeData(gyro);    //读取陀螺仪


    float Y_Acc =acc[1];      //(Acc, 1);                             // 获取向前的加速度(digite)
    float Z_Acc =acc[2];       // (Acc, 2);                             // 获取向下的加速度(digite)
    float angleA = atan(Y_Acc / Z_Acc) * 180 / pi;          // 根据加速度分量得到的角度(degree)
    float omega =  Gyr_Gain * (gyro[1]+  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);
}
复制代码