直流减速电机370 不带码盘---平衡小车

向阳

电机选型
电压：6V;空载转速：220转/分钟
空载电流0.08A；额定力矩：1.1Kg.cm

附上代码，站不起来，请大家帮忙看看

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Timer.h"//时间操作系统头文件  本程序用作timeChange时间采集并处理一次数据

Timer t;//时间类

float timeChange=20;//滤波法采样时间间隔毫秒
float dt=timeChange*0.001;//注意：dt的取值为滤波器采样时间
// 陀螺仪
float angleAx,gyroGy;//计算后的角度（与x轴夹角）和角速度
MPU6050 accelgyro;//陀螺仪类
int16_t ax, ay, az, gx, gy, gz;//陀螺仪原始数据 3个加速度+3个角速度

//一阶滤波
float K1 =0.05; // 对加速度计取值的权重
//float dt=20*0.001;//注意：dt的取值为滤波器采样时间
float angle1;//一阶滤波角度输出
//二阶滤波
float K2 =0.2; // 对加速度计取值的权重
float x1,x2,y1;//运算中间变量
//float dt=20*0.001;//注意：dt的取值为滤波器采样时间
float angle2;//er阶滤波角度输出

//卡尔曼滤波参数与函数
float angle, angle_dot;//角度和角速度
float angle_0, angle_dot_0;//采集来的角度和角速度
//float dt=20*0.001;//注意：dt的取值为kalman滤波器采样时间
//一下为运算中间变量
float P[2][2] = {{ 1, 0 },
              { 0, 1 }};
float Pdot[4] ={ 0,0,0,0};
float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度
float R_angle=0.5 ,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
////
unsigned long now ;
unsigned long preTime = 0;
//float angleG = 0;

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

float LSpeed_Need=0.0,RSpeed_Need=0.0;// 转弯设置
///
int TN1=3;
int TN2=5;
int ENA=6;
int TN3=9;
int TN4=10;
int ENB=11;

void setup() {
  
    Wire.begin();//初始化
    Serial.begin(9600);//初始化
    accelgyro.initialize();//初始化
    delay(10);
      pinMode(TN1,OUTPUT);         
      pinMode(TN2,OUTPUT);
      pinMode(TN3,OUTPUT);
      pinMode(TN4,OUTPUT);
      pinMode(ENA,OUTPUT);
      pinMode(ENB,OUTPUT);   

    int tickEvent1=t.every(timeChange, getangle);//本语句执行以后timeChange毫秒执行回调函数getangle

//    int tickEvent2=t.every(50, printout) ;//本语句执行以后50毫秒执行回调函数printout，串口输出
}

void loop() {
    t.update();//时间操作系统运行
}

void printout()
{
    Serial.print(angleAx);Serial.print(',');
    Serial.print(angle1);Serial.print(',');
    Serial.print(Output);Serial.println(',');
    // Serial.print(gx/131.00);Serial.print(',');
    //Serial.println(angle);//Serial.print(',');
}


void getangle()
{
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取原始6个数据
    angleAx=atan2(ax,az)*180/PI;//计算与x轴夹角
    gyroGy=-gy/131.00;//计算角速度
    Yijielvbo(angleAx,gyroGy);//一阶滤波
    PIDD();
    if (abs(angle1)<45)
    {    // 角度小于45度 运行程序
        //---------------------互补滤波+PID--------------------------        
        PWMB(); //----------------------PWM调速输出--------------------------
     }
      else
      {      // 角度大于45度 停止PWM输出
        analogWrite(ENA, 0);
        analogWrite(ENB, 0);        
      }
}

void PIDD()
{
    now = millis();                                         // 当前时间(ms)
    float TimeChange = (now - lastTime) / 1000.0;               // 采样时间(s)
    float Kp = 10.0, Ki = 0.23, Kd = 0.0;                    // 比例系数、积分系数、微分系数  //这里要如何调整?????????????
    float SampleTime = 0.1;                                 // 采样时间(s)
    float Setpoint =2.3;                                     // 设定目标值(degree)
    //float outMin = -255.0, outMax = +255.0;                   // 输出上限、输出下限
    if(TimeChange >= SampleTime)
    {                              // 到达预定采样时间时
        //float Input = angleG;                              // 输入赋值
        float Input = angle1;  
        float error = Setpoint - Input;                     // 偏差值
        ITerm+= (Ki * error * TimeChange);                      // 计算积分项
       // ITerm = constrain(ITerm, outMin, outMax);           // 限定值域
        float DTerm = Kd * (Input - lastInput) / TimeChange;    // 计算微分项
        Output = Kp * error + ITerm + DTerm;                // 计算输出值
        //Output = constrain(Output,outMin, outMax);         // 限定值域
        LOutput=Output+RSpeed_Need;                         //左电机
        ROutput=Output-RSpeed_Need;                         //右电机
        lastInput = Input;                                  // 记录输入值
        lastTime = now;                                     // 记录本次时间
    }
}

void Yijielvbo(float angle_m, float gyro_m)
{
    angle1 = K1 * angle_m+ (1-K1) * (angle1 + gyro_m * dt);
}

void Erjielvbo(float angle_m,float gyro_m)
{
    x1=(angle_m-angle2)*(1-K2)*(1-K2);
    y1=y1+x1*dt;
    x2=y1+2*(1-K2)*(angle_m-angle2)+gyro_m;
    angle2=angle2+ x2*dt;
}

void Kalman_Filter(double angle_m,double gyro_m)
{
    angle+=(gyro_m-q_bias) * dt;
    angle_err = angle_m - angle;
    Pdot[0]=Q_angle - P[0][1] - P[1][0];
    Pdot[1]=- P[1][1];
    Pdot[2]=- P[1][1];
    Pdot[3]=Q_gyro;
    P[0][0] += Pdot[0] * dt;
    P[0][1] += Pdot[1] * dt;
    P[1][0] += Pdot[2] * dt;
    P[1][1] += Pdot[3] * dt;
    PCt_0 = C_0 * P[0][0];
    PCt_1 = C_0 * P[1][0];
    E = R_angle + C_0 * PCt_0;
    K_0 = PCt_0 / E;
    K_1 = PCt_1 / E;
    t_0 = PCt_0;
    t_1 = C_0 * P[0][1];
    P[0][0] -= K_0 * t_0;
    P[0][1] -= K_0 * t_1;
    P[1][0] -= K_1 * t_0;
    P[1][1] -= K_1 * t_1;
    angle += K_0 * angle_err; //最优角度
    q_bias += K_1 * angle_err;
    angle_dot = gyro_m-q_bias;//最优角速度
}

void PWMB()
{
    if(LOutput>20)//左电机-------或者取0
    {   
        digitalWrite(TN1, HIGH);
        digitalWrite(TN2, LOW);
    }
      else if(LOutput<-20)//-------或者取0
    {
        digitalWrite(TN1, LOW);
        digitalWrite(TN2, HIGH);
    }
      else  //刹车-------取0后可以不用
    {
        digitalWrite(TN1, HIGH);
        digitalWrite(TN2, HIGH);
    }
    if(ROutput>20)//右电机--------或者取0
    {
        digitalWrite(TN3, HIGH);
        digitalWrite(TN4, LOW);     
    }
    else if(ROutput<-20)//-------或者取0
    {
        digitalWrite(TN3, LOW);
        digitalWrite(TN4, HIGH);  
    }
    else//刹车-------取0后可以不用
    {
        digitalWrite(TN3, HIGH);
        digitalWrite(TN4, HIGH);  
    }
    analogWrite(ENA,min(255,abs(LOutput)+25)); //PWM调速a==0-255
    analogWrite(ENB,min(255,abs(ROutput)+23));
}


分别尝试了一届和卡尔曼滤波，反应速度都还是可以的。因为是直流电机没有带码盘，总觉得PID算法上有问题，但是不知道怎么改
float Kp = 10.0, Ki = 0.23, Kd = 0.0;                    // 比例系数、积分系数、微分系数  //这里要如何调整?????????????

向阳

他的PID控制转速，pWM范围只能是0-255.大神帮忙看看啊。。。