|
先描述一下我的问题吧,开机一段时间后,大概有半分钟这样电机就开始失控,原因就是MPU6050停止工作,但是分开单独控制读取MOU6050的数据完全没有毛病,每当加上电机都是最多几分钟电机就失控了,电机继续以最后得到的数据跑,MPU6050停止工作。附上我的程序和硬件连接图,被这个问题困扰了好几天了,大家有什么好的建议可以随便说哈,有什么问题也可以问我,我尽我能力去回答。
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
//-------------------函数声明------------------
void fifter_pid(void);
void Motor(void);
//------------------mpu6050变量定义---------------
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
float Angle;
float omega;
//-----------------------引脚定义-------------------------
#define E_left 9
#define E_right 10
#define M_left1 4
#define M_left2 5
#define M_right1 6
#define M_right2 7
#define Pi 3.14159
#define Gry_offset 170 // 陀螺仪偏移量
#define Gyr_Gain 0.00763358 //灵敏度131
//------------------------------------pid---------------------------
float SampleTime=0.1; //PID 采样时间0.1 s
unsigned long lastime = 0;
float angle_output;
float Setpoint=0.0, Input;
float kp;
float ki;
float kd;
float PTerm, ITerm, DTerm;
float error, last_error, errSum, dErr;
//-----------------------一阶互补滤波--------------------
unsigned long pretime = 0, now;
float f_angle = 0.0; //融合值
//-------------------电机输出定义----------------------
float pwm_left, pwm_right;
float Turn_speed = 0.0; //转弯差速
void setup()
{
//-----------------初始化mpu6050-------------------
Wire.begin();
accelgyro.initialize();
pinMode(E_left, OUTPUT);
pinMode(E_right, OUTPUT);
pinMode(M_left1, OUTPUT);
pinMode(M_left2, OUTPUT);
pinMode(M_right1, OUTPUT);
pinMode(M_right2, OUTPUT);
Serial.begin(9600);
}
void loop()
{
//-------------------------采集原始数据-----------------
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Angle = atan2(ax,az) * 180 / Pi;
omega = -Gyr_Gain * (gx + Gry_offset);// 当前角速度(degree/s)
Serial.print(Angle);Serial.print('\t');
Serial.print(omega);Serial.print('\t');
if(abs(Angle) < 35)
{
fifter_pid();
Motor();
}
else
{
analogWrite(E_left, 0);
analogWrite(E_right, 0);
}
delay(5);
}
void fifter_pid()
{
//---------------------一阶互补滤波-------------------
now = millis(); // 当前时间(ms),millis返回的数据为长整形
float dt = (now - pretime) / 1000.0; // 微分时间(ms),除以1000换成s,dt的取值为滤波器采样时间
pretime = now;
float K = 0.8;
float A = K / (K + dt);
f_angle = A * (f_angle + omega * dt) + (1 - A) * Angle; // 互补滤波算法
Serial.println(f_angle);
//-----------------------实现定时器功能,0.1s进入一次----------
kp=15.0;
ki=0.0;
kd=0.0;
now = millis();
float timeChange = (now - lastime) / 1000.0;
if(timeChange >= SampleTime)
{
Input = f_angle;
error = Setpoint - Input;
errSum = errSum + error * timeChange;
dErr = (error - last_error) / timeChange;
PTerm = kp * error;
ITerm = ki * errSum;
ITerm = constrain(ITerm, -255.0, 255.0); // 限定值域
DTerm = kd * dErr;
angle_output = PTerm + ITerm + DTerm;
angle_output = constrain(angle_output, -255.0, 255.0);
//--------------------------差速控制-----------------------------
pwm_left = angle_output + Turn_speed;
pwm_right = angle_output - Turn_speed;
last_error = error;
lastime = now;
}
}
void Motor()
{
if(pwm_left > 0)
{
digitalWrite(M_left1, LOW);
digitalWrite(M_left2, HIGH);
}
else if(pwm_left < 0)
{
digitalWrite(M_left1, HIGH);
digitalWrite(M_left2, LOW);
}
if(pwm_right > 0)
{
digitalWrite(M_right1,LOW);
digitalWrite(M_right2,HIGH);
}
else if(pwm_right < 0)
{
digitalWrite(M_right1,HIGH);
digitalWrite(M_right2,LOW);
}
int nLeftVol = abs(pwm_left);
int nRightVol = abs(pwm_right);
// Serial.print(nLeftVol);Serial.print('\t');
// Serial.println(nRightVol);
analogWrite(E_left, nLeftVol);
analogWrite(E_right, nRightVol);
}
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
|