极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 19274|回复: 7

自平衡小车,电机控制失控问题

[复制链接]
发表于 2017-8-27 16:54:40 | 显示全部楼层 |阅读模式
先描述一下我的问题吧,开机一段时间后,大概有半分钟这样电机就开始失控,原因就是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
回复

使用道具 举报

发表于 2017-8-28 08:39:33 | 显示全部楼层
楼主好,我猜想是某个元件过热所至,如:MPU6050。
回复 支持 反对

使用道具 举报

发表于 2017-8-28 17:46:15 | 显示全部楼层
居然上2560,土豪啊,是散热问题还是电机火花干扰?这两方面去找找看
回复 支持 反对

使用道具 举报

发表于 2017-8-29 16:59:32 来自手机 | 显示全部楼层
电机干扰。我用N20没问题,换TT就不行了。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2017-8-30 10:44:56 | 显示全部楼层
太行摄狼 发表于 2017-8-28 17:46
居然上2560,土豪啊,是散热问题还是电机火花干扰?这两方面去找找看

MEGA功能强大点,散热应该没什么问题的,至于电火花还真没看出来
回复 支持 反对

使用道具 举报

 楼主| 发表于 2017-8-30 10:46:31 | 显示全部楼层
kpj001 发表于 2017-8-29 16:59
电机干扰。我用N20没问题,换TT就不行了。

这问题我也想过,可是换了个带编码器的电机也还是那样之后我就开始怀疑人生了
回复 支持 反对

使用道具 举报

 楼主| 发表于 2017-8-30 10:47:16 | 显示全部楼层
独行者 发表于 2017-8-28 08:39
楼主好,我猜想是某个元件过热所至,如:MPU6050。

嗯。。好像并没怎么发热?
回复 支持 反对

使用道具 举报

发表于 2019-4-11 15:29:09 | 显示全部楼层
应该是12v电源直接供给mega2560的原因,加个8v以下的降压模块可能会解决问题
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2024-3-29 22:15 , Processed in 0.049073 second(s), 23 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表