极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 40036|回复: 15

Arduino控制直流电机和MPU6050结合

[复制链接]
发表于 2014-8-26 09:40:40 | 显示全部楼层 |阅读模式
本帖最后由 liangquan 于 2014-8-26 12:14 编辑

我的想法是检测MPU6050中X轴的倾斜角度,当绕X轴转角为+时,电机正转;当绕X轴的倾角为-时,电机反转。

电路图如下图所示:


我采用了两套方法来读取MPU6050的倾斜角度,用程序单独读角度,都可以得到正确的角度结果。如果我单独控制电机,也可以控制电机的正反转,但是当我将两者结合时,电机就只能向一个方向旋转,而不能实现根据角度的不同,转向不同。

配置如下:
1)Arduino Uno R3
2)检测小车姿态传感器,MPU-6050,(用了网上一个外国人人开发的类库,经单独测试,可以读出芯片的倾斜角度)
3)L293D,控制小车的两个车轮,(目前我想先实现电机的正反转,再说调速的问题);
4)USB数据线或9V电池为Arduino供电,6节1.5V5#电池为小车直流电机供电;

程序如下:
#include <Wire.h>
#include <I2Cdev.h>
#include <MPU6050.h>

unsigned long t = 0;  // Time Variables

#define detT 10    // time difference in milli seconds

// DC motor
const int motor1Pin = 3;
const int motor2Pin = 4;
const int enablePin = 9;

const int ledPin = 10;

/////////////////////////////////////////
// MPU6050
MPU6050 accelgyro;//mpu6050数据定义
int16_t ax, ay, az;
int16_t gx, gy, gz;

//******角度参数************
float Gyro_X;        //X轴陀螺仪数据暂存
float Angle;         //小车最终倾斜角度
char value;          //角度正负极性标记       
float arctanangle;   //反正切角度

//******卡尔曼参数************
static const double C_0 = 1;
static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.01;//注意:dt的取值为kalman滤波器采样时间
double P[2][2] = {{ 1, 0 },
                  { 0, 1 }};
double Pdot[4] ={ 0,0,0,0};
double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
double angle, angle_dot;

//------------------------------------------------------------------
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 setup()
{
  Wire.begin();
  //Serial.begin(115200);
  //Serial.println("Initializing I2C devices...");
  accelgyro.initialize();
  //Serial.println("Testing device connections...");
  //Serial.println(accelgyro.testConnection()?"MPU6050 connection successful" : "MPU6050 connection failed");
  
  // DC motor
  pinMode(motor1Pin, OUTPUT);
  pinMode(motor2Pin, OUTPUT);
  pinMode(enablePin, OUTPUT);
  
  pinMode(ledPin, OUTPUT);
  
  // set enablePin high so that motor can turn on
  digitalWrite(enablePin, HIGH);
  digitalWrite(motor1Pin, LOW);
  digitalWrite(motor2Pin, LOW);
}
void loop()
{
  t = millis();
  
  angle_calculate();
  
  if (angle > 0)
  {
    digitalWrite(motor1Pin, LOW);
    digitalWrite(motor2Pin, HIGH);

    digitalWrite(ledPin, HIGH);
  }
  else
  {
    digitalWrite(motor1Pin, HIGH);
    digitalWrite(motor2Pin, LOW);

    digitalWrite(ledPin, LOW);
  }
  
  while((millis()-t) < detT)
  {
    // Do nothing
  }
}

void angle_calculate()
{
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  Gyro_X = (gx+400)/131.0;
  arctanangle=atan2(ay+1200, az)*180/3.14;
  Kalman_Filter(arctanangle,Gyro_X);
  
  //Serial.print(angle);
  //Serial.print("\n");
}

如果注释掉红色的部分,一切正常,倾角为正、负时可以控制LED的亮、灭。但是当电机一旋转,马上就检测不到角度了,我觉得好像是程序不兼容,要么是硬件接线有问题,请朋友们帮我分析一下原因,谢谢!!!

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复

使用道具 举报

 楼主| 发表于 2014-9-1 09:47:34 | 显示全部楼层
ianon 发表于 2014-8-29 13:01
之前没细看,不好意思
查了下,293的16脚是电路的供电电压, 8教是芯片的工作电压
我认为8脚该接到5V,16接 ...

我查的结果是这样的:


应该是16引脚是芯片的供电5V电压吧?8引脚是电机的供电电压吧?

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复 支持 1 反对 0

使用道具 举报

发表于 2014-8-26 12:46:48 | 显示全部楼层
电源的可能性很大,电机一转,就没有5v了,mpu6050就挂了。
回复 支持 反对

使用道具 举报

发表于 2014-8-26 13:22:45 | 显示全部楼层
本帖最后由 林定祥 于 2014-8-26 13:26 编辑

可以读一下MPU6050的其他参数看看是否受到电源或通讯线路干扰.电机起来了会有较强的电磁干扰的.
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-8-26 16:02:12 | 显示全部楼层
yyy_zc 发表于 2014-8-26 12:46
电源的可能性很大,电机一转,就没有5v了,mpu6050就挂了。

MPU6050的电源是3.3V吧?

那您看我的电路得怎样改进呢?怎样保证MPU6050的3.3V不降低呢?增大电机的供电电压?将Arduino和电机供电的地隔离?可怎样隔离呢?
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-8-26 16:05:11 | 显示全部楼层
林定祥 发表于 2014-8-26 13:22
可以读一下MPU6050的其他参数看看是否受到电源或通讯线路干扰.电机起来了会有较强的电磁干扰的.

我觉得干扰的可能性低。不是数据乱了,是完全的停止读取了,就好像MPU6050停止工作了。我的想法是做一个自平衡小车。那些做自平衡小车的前辈们也碰到过干扰问题么?怎样解决干扰问题呢?我觉得干扰可能性不大……
回复 支持 反对

使用道具 举报

发表于 2014-8-26 17:59:03 | 显示全部楼层
双路直流电源。。。。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-8-26 19:10:26 | 显示全部楼层
本帖最后由 liangquan 于 2014-8-26 19:11 编辑
ianon 发表于 2014-8-26 17:59
双路直流电源。。。。


你是说用“双路直流稳压电源”?有多大?我要做自平衡小车,这个电源一起带着?不会吧?

淘宝上搜索了一下,体积大,价格贵,不是我的目的
回复 支持 反对

使用道具 举报

发表于 2014-8-26 19:53:03 | 显示全部楼层
电机的电源和6050的电源分开,分别供电。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-8-26 20:12:38 | 显示全部楼层
nust_奔跑 发表于 2014-8-26 19:53
电机的电源和6050的电源分开,分别供电。

谢谢您的回复。

见电路图:电机的电源是由与L293D的8引脚相连的干电池提供的,MPU6050的电源是由Arduino的3.3V引脚提供的,是不是已经“分别”供电了?但现在的问题是他们公用一个地,是不是这的问题?
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-8-26 20:13:07 | 显示全部楼层
nust_奔跑 发表于 2014-8-26 19:53
电机的电源和6050的电源分开,分别供电。

谢谢您的回复。

见电路图:电机的电源是由与L293D的8引脚相连的干电池提供的,MPU6050的电源是由Arduino的3.3V引脚提供的,是不是已经“分别”供电了?但现在的问题是他们公用一个地,是不是这的问题?
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-8-26 20:19:27 | 显示全部楼层
从网上找到一个老外的帖子

CONTROL DC MOTOR CW/CCW WITH MPU-6050 GYRO/ACCELEROMETER + ARDUINO

In this article you will get the code and circuit diagram to control the DC Motor CW/CCW using GY-521 gyroscope and accelerometer module (MPU-6050).

To make this prototype I am using:

Arduino UNO
GY-521 (MPU-6050)
L29dD Driver IC
DC Motor
Breadboard
Jumper wire



程序:
  1. #include <Wire.h>
  2. #include<I2Cdev.h>
  3. #include<MPU6050.h>

  4. MPU6050 mpu;

  5. int16_t ax, ay, az;
  6. int16_t gx, gy, gz;

  7. #define pin1 3
  8. #define pin2 5

  9. void setup(){
  10. Serial.begin(9600);
  11. Serial.println("Initialize MPU");
  12. mpu.initialize();
  13. //Serial.println(mpu.testConnection() ? "Connected" : "Connection failed"); pinMode(pin1,OUTPUT);
  14. pinMode(pin2,OUTPUT);

  15. }

  16. void loop(){
  17. mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  18. ax = map(ax, -17000, 17000, -1500, 1500);

  19. //Serial.println(ax);
  20. if(ax > 0){
  21.   if(ax<255){
  22.   Serial.println(ax);
  23.   analogWrite(pin2,ax);
  24. }
  25. else{
  26.   Serial.println("+255");
  27.   analogWrite(pin2,255);
  28. }

  29. }
  30. if(ax<0){
  31.   if(ax>-255){
  32.   Serial.println(ax);
  33.   analogWrite(pin1, ax-ax-ax);
  34. }
  35. else{
  36.   Serial.println("-255");
  37.   analogWrite(pin1, 255);
  38. }
  39. }
  40. delay(1000);
  41. }
复制代码
有这样几个问题:
1)程序中没有对9引脚编程,怎么能控制电机?
2)这个例子,电机、MPU、Arduino是共地;
3)“analogWrite(pin1, ax-ax-ax);”为什么要写“ax-ax-ax”,我认为写一个ax就够了。

现在在家,无法调试,明天测试!!!

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复 支持 反对

使用道具 举报

发表于 2014-8-29 13:01:03 | 显示全部楼层
之前没细看,不好意思
查了下,293的16脚是电路的供电电压, 8教是芯片的工作电压
我认为8脚该接到5V,16接到电池,这样才能让电池给电机供电

或者8和16都接到电池,293的供电电压范围很大的,不一定要5V
回复 支持 反对

使用道具 举报

发表于 2016-10-5 15:14:45 | 显示全部楼层
遇到同样的问题,Arduino与电源共地之后虽然MPU6050电压仍是5v,但串口读不出数据。不知楼主解决了没?
回复 支持 反对

使用道具 举报

 楼主| 发表于 2016-11-10 10:52:05 | 显示全部楼层
星小白 发表于 2016-10-5 15:14
遇到同样的问题,Arduino与电源共地之后虽然MPU6050电压仍是5v,但串口读不出数据。不知楼主解决了没?

还没解决,期待高手回答
回复 支持 反对

使用道具 举报

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

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

Archiver|联系我们|极客工坊

GMT+8, 2024-3-29 05:14 , Processed in 0.051251 second(s), 25 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

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