前几天我在贴吧看了几个贴，就开始动手做自平衡小车。现在还有很多不懂，PID那几个参数我都是随机调的，每次调完就烧程序进板，没有借用任何什么串口监视之类的东西观察数据变化情况，只能看小车的运行情况，掌握不到大概思路是什么。
现在，硬件部分做好了，开始调程序，我的自平衡小车，刚开始时候（大概持续2s左右），还会在平衡位置不断摆动，但过后就一直往一个方向运动了。（知道的DX请告诉我一下）
一下是我的代码：
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
#define Gry_offset -20
#define Gyr_Gain 65
#define pi 3.14159
int16_t ax, ay, az;
int16_t gx, gy, gz;
/********** 互补滤波器参数 *********/
unsigned long preTime = 0; // 采样时间
float f_angle = 0.0; // 滤波处理后的角度值
unsigned long lastTime; // 前次时间
float ITerm, lastInput; // 积分项、前次输入
float Output = 0.0; // PID输出值
int oommm;
int TN1=3;
int TN2=4;
int TN3=8;
int TN4=7;
int ENA=5;
int ENB=9;
void setup()
{
Wire.begin();
Serial.begin(9600);
accelgyro.initialize();
delay(500);
pinMode(3,OUTPUT);
pinMode(4,OUTPUT);
pinMode(8,OUTPUT);
pinMode(7,OUTPUT);
pinMode(5,OUTPUT);
pinMode(9,OUTPUT);
delay(100);
}
void loop()
{
unsigned long now = millis(); // 当前时间(ms)
float dt = (now - preTime) / 1000.0; // 微分时间(s)
preTime = now; // 记录本次时间(ms)
float K = 0.8; // 取值权重
float A = K / (K + dt); // 加权系数
float TimeCh = (now - lastTime) / 1000.0; // 采样时间(s)
float Kp = 0.9, Ki = 26.0, Kd = 5.0；
// 比例系数、积分系数、微分系数
float SampleTime = 0.001; // 采样时间(s)
float Setpoint = -3.8; // 设定目标值(degree)
float outMin = -150.0, outMax = +150.0;
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
float angleA= atan2(ax , az) * 180 / pi;
float omega=(gy + Gry_offset)/Gyr_Gain; // 当前角速度(degree/s)
if (abs(angleA)<45) //这个是误差达到一定程度后的系统关闭开关
{
//Kalman_Filter(angleA,omega);
f_angle = A * (f_angle + omega * dt) + (1-A) * angleA;
now = millis();
if(TimeCh >= SampleTime) { // 到达预定采样时间时
float Input = f_angle; // 输入赋值
float error = Setpoint - Input; // 偏差值
ITerm+= (Ki * error * TimeCh); // 计算积分项
ITerm = constrain(ITerm, outMin, outMax); // 限定值域
float DTerm = Kd * (Input - lastInput) / TimeCh; // 计算微分项
Output = Kp * error + ITerm - DTerm; // 计算输出值
Output = constrain(Output, outMin, outMax); // 限定值域
PWMB();
lastInput = Input; // 记录输入值
lastTime = now;
}
else {
analogWrite(5, 0); //PWM调速a==0-255
analogWrite(9, 0);
}
delay(3);
}
}
//-------------电机正反转 PWM输出-----------
void PWMB()
{
if(Output<0)
{ /* digitalWrite(3, LOW);
digitalWrite(4, HIGH);
digitalWrite(8, LOW);
digitalWrite(7, HIGH);*/
digitalWrite(3, HIGH);
digitalWrite(4, LOW);
digitalWrite(8, HIGH);
digitalWrite(7, LOW);
}
else if(Output>0)
{
digitalWrite(3, LOW);
digitalWrite(4, HIGH);
digitalWrite(8, LOW);
digitalWrite(7, HIGH);
}
//oommm=min(220,abs(40*Output));
analogWrite(5, Output+35); //PWM调速a==0-220
analogWrite(9, Output+30);
}
这个程序我是结合两个人的程序写的，有几条我都不明白是怎么得来的：
1：
#define Gry_offset -20
#define Gyr_Gain 65
float Setpoint = -3.8; // 设定目标值(degree)
（这三个值是如何得到的，还有有什么作用）
2：陀螺仪的静态偏移量是怎么测的， |
发表于 2015-2-5 17:02:50
