arduino做的平衡车
本帖最后由 ypw 于 2014-8-14 23:27 编辑http://v.youku.com/v_show/id_XNzU1NzA0ODk2.html
#include <I2Cdev.h>
#include <MPU6050.h>
#include <Wire.h>
#define X_pianyi -6
float P = 40, D = 0.025;
//PID的P和D的值
//====================
MPU6050 accelgyro;
int DIR1 = 7, PWM1 = 6, PWM2 = 5, DIR2 = 4;
//dir代表方向，pwm代表速度
int16_t ax, ay, az;
int16_t gx, gy, gz; //存储原始数据
float aax, aay, aaz, ggx, ggy, ggz; //存储量化后的数据
float Ax, Ay, Az; //单位 g(9.8m/s^2)
float Gx, Gy, Gz; //单位 °/s
float Angel_accX, Angel_accY, Angel_accZ; //存储加速度计算出的角度
long LastTime, NowTime, TimeSpan; //用来对角速度积分的
#define LED_PIN 13//LED
bool blinkState = false;
void set(int a)
{
analogWrite(PWM2, abs(a));
if (a > 0)
{
digitalWrite(DIR1, HIGH);
digitalWrite(DIR2, HIGH);
}
else if (a < 0) {
digitalWrite(DIR1, LOW);
digitalWrite(DIR2, LOW);
}
else {
digitalWrite(PWM1, LOW);
digitalWrite(PWM2, LOW);
}
}
void setup()//MPU6050的设置都采用了默认值，请参看库文件
{
digitalWrite(PWM1, LOW);
digitalWrite(PWM2, LOW);
pinMode(DIR1, OUTPUT);
pinMode(DIR2, OUTPUT);
pinMode(PWM1, OUTPUT);
pinMode(PWM2, OUTPUT);
digitalWrite(DIR1, LOW);
digitalWrite(DIR2, LOW);
pinMode(LED_PIN, OUTPUT);
delay(500);
Wire.begin();
Serial.begin(57600);
Serial.println("Initializing I2C device.....");
accelgyro.initialize();
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failure");
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Ax = ax / 16384.00;
Ay = ay / 16384.00;
Az = az / 16384.00;
Angel_accZ = atan(Az / sqrt(Ax * Ax + Ay * Ay)) * 180 / 3.14;
Gx = -Angel_accZ;
Serial.println(Gx);
if (!(Gx < 180 && Gx > -180))setup();
digitalWrite(13,1);
}
void loop()
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //获取三个轴的加速度和角速度
//======一下三行是对加速度进行量化，得出单位为g的加速度值
Ax = ax / 16384.00;
Ay = ay / 16384.00;
Az = az / 16384.00;
//==========以下三行是用加速度计算三个轴和水平面坐标系之间的夹角
Angel_accX = atan(Ax / sqrt(Az * Az + Ay * Ay)) * 180 / 3.14;
Angel_accY = atan(Ay / sqrt(Ax * Ax + Az * Az)) * 180 / 3.14;
Angel_accZ = atan(Az / sqrt(Ax * Ax + Ay * Ay)) * 180 / 3.14;
//==========以下三行是对角速度做量化==========
ggx = gx / 131.00;
ggy = gy / 131.00;
ggz = gz / 131.00;
//===============以下是对角度进行积分处理====== ==========
NowTime = millis(); //获取当前程序运行的毫秒数
TimeSpan = NowTime - LastTime; //积分时间这样算不是很严谨
//下面是互补滤波算法
Gx = (Gx + ggx * TimeSpan / 1000)*0.995-0.005*Angel_accZ;
LastTime = NowTime;
int spd;
spd = (Gx - X_pianyi) * P - ggx * D;
if (spd < 250 && spd > -250)set(spd);
else {
set(0);
}
//==============================
Serial.print("Ax=");
Serial.print(Gx);
Serial.print("\tWx=");
Serial.print(ggx);
Serial.print("\tspd=");
Serial.println(spd);
} 做例子不错。我昨儿个也想到这种积分方法，我觉得中断影响MPU6050，一开电机那个角度就飘到爪哇国去了 这个程序看上去很精练,不知道效果如何呢? 视频看的 挺好的。
:lol:):) 这个不错 电机用码盘没有呀？ PLA 发表于 2014-8-23 09:45 static/image/common/back.gif
电机用码盘没有呀？
没有，直接转圈平衡的 楼主能看看我的这篇帖子么？
http://www.arduino.cn/thread-7049-1-1.html
我想做一个两轮自平衡小车。现在的问题是单独用MPU6050，可以得到角度，单独用L293，可以控制电机旋转，但是当两者一结合，电机就只向一个方向旋转。
我觉得是我用的那个老外的#include "gyro_accel.h"类有问题，链接如下：
http://hobbylogs.me.pn/?p=47
和控制电机冲突。
我的你的获取MPU6050的角度的方法好像很简单，但是我看不明白，楼主能给出一个单独使用MPU6040获取角度的程序么？你使用了滤波算法么？ liangquan 发表于 2014-8-25 16:34 static/image/common/back.gif
楼主能看看我的这篇帖子么？
http://www.arduino.cn/thread-7049-1-1.html
我直接用了MPU6050的库获取了加速度和角速度，然后用互补滤波算法得到了角度。
Gx = (Gx + ggx * TimeSpan / 1000)*0.995-0.005*Angel_accZ;
然后用PID控制
spd = (Gx - X_pianyi) * P - ggx * D; ypw 发表于 2014-8-27 12:52 static/image/common/back.gif
我直接用了MPU6050的库获取了加速度和角速度，然后用互补滤波算法得到了角度。
Gx = (Gx + ggx * Time ...
楼主能看看我的这篇帖子么？
http://www.geek-workshop.com/thread-10818-1-1.html
我现在的问题是：只要Arduino和MPU一结合，就不能按照我的想法去运行。您看看我的电路图有毛病么？您碰到过类似问题么？
我用11#楼的做演示，也不一定成功，这是怎么回事？ liangquan 发表于 2014-8-27 14:03 static/image/common/back.gif
楼主能看看我的这篇帖子么？
http://www.geek-workshop.com/thread-10818-1-1.html
我的电机和单片机是分开的电源,单片机用移动电源5V供电,功耗很低,电机用7.2V的镍氢电池供电,飞思卡尔车模的标配,由于电机的电源跟单片机是不一样的,所以没有这个不工作的问题.至于你那个具体是什么问题我也没看出来.
页:
[1]