Arduino控制直流电机和MPU6050结合
本帖最后由 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 = {{ 1, 0 },
{ 0, 1 }};
double Pdot ={ 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=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 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的亮、灭。但是当电机一旋转，马上就检测不到角度了，我觉得好像是程序不兼容，要么是硬件接线有问题，请朋友们帮我分析一下原因，谢谢！！！
ianon 发表于 2014-8-29 13:01 static/image/common/back.gif
之前没细看，不好意思
查了下，293的16脚是电路的供电电压, 8教是芯片的工作电压
我认为8脚该接到5V，16接 ...
我查的结果是这样的：
应该是16引脚是芯片的供电5V电压吧？8引脚是电机的供电电压吧？ 电源的可能性很大，电机一转，就没有5v了，mpu6050就挂了。 本帖最后由 林定祥 于 2014-8-26 13:26 编辑
可以读一下MPU6050的其他参数看看是否受到电源或通讯线路干扰.电机起来了会有较强的电磁干扰的. yyy_zc 发表于 2014-8-26 12:46 static/image/common/back.gif
电源的可能性很大，电机一转，就没有5v了，mpu6050就挂了。
MPU6050的电源是3.3V吧？
那您看我的电路得怎样改进呢？怎样保证MPU6050的3.3V不降低呢？增大电机的供电电压？将Arduino和电机供电的地隔离？可怎样隔离呢？ 林定祥 发表于 2014-8-26 13:22 static/image/common/back.gif
可以读一下MPU6050的其他参数看看是否受到电源或通讯线路干扰.电机起来了会有较强的电磁干扰的.
我觉得干扰的可能性低。不是数据乱了，是完全的停止读取了，就好像MPU6050停止工作了。我的想法是做一个自平衡小车。那些做自平衡小车的前辈们也碰到过干扰问题么？怎样解决干扰问题呢？我觉得干扰可能性不大…… 双路直流电源。。。。 本帖最后由 liangquan 于 2014-8-26 19:11 编辑
ianon 发表于 2014-8-26 17:59 static/image/common/back.gif
双路直流电源。。。。
你是说用“双路直流稳压电源”？有多大？我要做自平衡小车，这个电源一起带着？不会吧？
淘宝上搜索了一下，体积大，价格贵，不是我的目的 电机的电源和6050的电源分开，分别供电。 nust_奔跑 发表于 2014-8-26 19:53 static/image/common/back.gif
电机的电源和6050的电源分开，分别供电。
谢谢您的回复。
见电路图：电机的电源是由与L293D的8引脚相连的干电池提供的，MPU6050的电源是由Arduino的3.3V引脚提供的，是不是已经“分别”供电了？但现在的问题是他们公用一个地，是不是这的问题？ nust_奔跑 发表于 2014-8-26 19:53 static/image/common/back.gif
电机的电源和6050的电源分开，分别供电。
谢谢您的回复。
见电路图：电机的电源是由与L293D的8引脚相连的干电池提供的，MPU6050的电源是由Arduino的3.3V引脚提供的，是不是已经“分别”供电了？但现在的问题是他们公用一个地，是不是这的问题？ 从网上找到一个老外的帖子
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
程序：#include <Wire.h>
#include<I2Cdev.h>
#include<MPU6050.h>
MPU6050 mpu;
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define pin1 3
#define pin2 5
void setup(){
Serial.begin(9600);
Serial.println("Initialize MPU");
mpu.initialize();
//Serial.println(mpu.testConnection() ? "Connected" : "Connection failed"); pinMode(pin1,OUTPUT);
pinMode(pin2,OUTPUT);
}
void loop(){
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
ax = map(ax, -17000, 17000, -1500, 1500);
//Serial.println(ax);
if(ax > 0){
if(ax<255){
Serial.println(ax);
analogWrite(pin2,ax);
}
else{
Serial.println("+255");
analogWrite(pin2,255);
}
}
if(ax<0){
if(ax>-255){
Serial.println(ax);
analogWrite(pin1, ax-ax-ax);
}
else{
Serial.println("-255");
analogWrite(pin1, 255);
}
}
delay(1000);
}有这样几个问题：
1）程序中没有对9引脚编程，怎么能控制电机？
2）这个例子，电机、MPU、Arduino是共地；
3）“analogWrite(pin1, ax-ax-ax);”为什么要写“ax-ax-ax”，我认为写一个ax就够了。
现在在家，无法调试，明天测试！！！ 之前没细看，不好意思
查了下，293的16脚是电路的供电电压, 8教是芯片的工作电压
我认为8脚该接到5V，16接到电池，这样才能让电池给电机供电
或者8和16都接到电池，293的供电电压范围很大的，不一定要5V 遇到同样的问题，Arduino与电源共地之后虽然MPU6050电压仍是5v，但串口读不出数据。不知楼主解决了没？ 星小白 发表于 2016-10-5 15:14
遇到同样的问题，Arduino与电源共地之后虽然MPU6050电压仍是5v，但串口读不出数据。不知楼主解决了没？
还没解决，期待高手回答
页:
[1]
2