|
本帖最后由 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
|