直流减速电机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 = {{ 1, 0 },
{ 0, 1 }};
float Pdot ={ 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=Q_angle - P - P;
Pdot=- P;
Pdot=- P;
Pdot=Q_gyro;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
PCt_0 = C_0 * P;
PCt_1 = C_0 * P;
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;
P -= K_0 * t_0;
P -= K_0 * t_1;
P -= K_1 * t_0;
P -= 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.大神帮忙看看啊。。。
页:
[1]