Arduino自平衡小车……你值得拥有

ID:100312 · 发表于 2016-4-9 10:44

#include "Wire.h"`
const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
const uint16_t I2C_TIMEOUT = 100; // Used to check for errors in I2C communication
uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success
}
uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data, length);
uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
if (rcode) {
Serial.print(F("i2cWrite failed: "));
Serial.println(rcode);
}
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}
uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
uint32_t timeOutTimer;
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
if (rcode) {
Serial.print(F("i2cRead failed: "));
Serial.println(rcode);
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}
Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading
for (uint8_t i = 0; i < nbytes; i++) {
if (Wire.available())
data[i] = Wire.read();
else {
timeOutTimer = micros();
while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
if (Wire.available())
data[i] = Wire.read();
else {
Serial.println(F("i2cRead timeout"));
return 5; // This error value is not already taken by endTransmission
}
}
}
return 0; // Success
}
/******************************************************/
//2560 pin map 引脚定义好即可，然后改变一下PID的几个值（kp，kd，ksp，ksi）即可，剩下的全部都是固定的程序，
//可能小车会有一点重心不在中点的现象，加一下角度值或者减一点即可
//至于每个MPU6050的误差，自己调节一下即可，不是很难
//调试时先将速度环的ksp，ksi=0，调到基本可以站起来，然后可能会出现倒，或者自动跑起来的时候加上速度环
//这时就会很稳定的站起来，然后用小力气的手推不会倒。
int ENA=11;
int ENB=12;
int IN1=4;
int IN2=5;
int IN3=6;
int IN4=7;
int MAS,MBS;
/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;
double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
uint8_t i2cData[14]; // Buffer for I2C data
uint32_t timer;
unsigned long lastTime;
/***************************************/
double P[2][2] = {{ 1, 0 },{ 0, 1 }};
double Pdot[4] ={ 0,0,0,0};
static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5,dtt=0.005,C_0 = 1;
double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
double angle,angle_dot,aaxdot,aax;
double position_dot,position_dot_filter,positiono;
/*-------------Encoder---------------*/
#define LF 0
#define RT 1
//The balance PID
float kp,kd,ksp,ksi;
int Lduration,Rduration;
boolean LcoderDir,RcoderDir;
const byte encoder0pinA = 2;
const byte encoder0pinB = 8;
byte encoder0PinALast;
const byte encoder1pinA = 3;
const byte encoder1pinB = 9;
byte encoder1PinALast;
int RotationCoder[2];
int turn_flag=0;
float move_flag=0;
float right_need = 0, left_need = 0;;
int pwm;
int pwm_R,pwm_L;
float range;
float range_error_all;
float wheel_speed;
float last_wheel;
float error_a=0;
void Kalman_Filter(double angle_m,double gyro_m)
{
angle+=(gyro_m-q_bias) * dtt;
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] * dtt;
P[0][1] += Pdot[1] * dtt;
P[1][0] += Pdot[2] * dtt;
P[1][1] += Pdot[3] * dtt;
angle_err = angle_m - angle;
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;//也许应该用last_angle-angle
}
void setup() {
Wire.begin();
Serial.begin(9600);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
delay(20); // Wait for sensor to stabilize
while (i2cRead(0x3B, i2cData, 6));
accX = (i2cData[0] << 8) | i2cData[1];
accY = (i2cData[2] << 8) | i2cData[3];
accZ = (i2cData[4] << 8) | i2cData[5];
double roll = atan2(accX, accZ) * RAD_TO_DEG;
EnCoderInit();
timer = micros();
//The balance PID
kp= 42;//24.80;43
kd= 1.9;//9.66;1.4
ksp= 8.5;//4.14;
ksi= 2.1;//0.99; 0.550
}
void EnCoderInit()
{
pinMode(8,INPUT);
pinMode(9,INPUT);
attachInterrupt(LF, LwheelSpeed, RISING);
attachInterrupt(RT, RwheelSpeed, RISING);
}
void pwm_calculate()
{
unsigned long now = millis(); // 当前时间(ms)
int Time = now - lastTime;
int range_error;
range += (Lduration + Rduration) * 0.5;
range *= 0.9;
range_error = Lduration - Rduration;
range_error_all += range_error;
wheel_speed = range - last_wheel;
//wheel_speed = constrain(wheel_speed,-800,800);
//Serial.println(wheel_speed);
last_wheel = range;
pwm = (angle + 0.825) * kp + angle_dot * kd + range * ksp + wheel_speed * ksi;
if(pwm > 255)pwm = 255;
if(pwm < -255)pwm = -255;
if(turn_flag == 0)
{
pwm_R = pwm + range_error_all;
pwm_L = pwm - range_error_all;
}
else if(turn_flag == 1) //左转
{
pwm_R = pwm ; //Direction PID control
pwm_L = pwm + left_need * 68;
range_error_all = 0; //clean
}
else if(turn_flag == 2)
{
pwm_R = pwm + right_need * 68; //Direction PID control
pwm_L = pwm ;
range_error_all = 0; //clean
}
Lduration = 0;//clean
Rduration = 0;
lastTime = now;
}
void PWMD()
{
if(pwm>0)
{
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}
else if(pwm<0)
{
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}
int PWMr = abs(pwm);
int PWMl = abs(pwm);
analogWrite(ENB, max(PWMl,60)); //PWM调速a==0-255 51
analogWrite(ENA, max(PWMr,60)); //PWM调速a==0-255 54
}
void LwheelSpeed()
{
if(digitalRead(encoder0pinB))
Lduration++;
else Lduration--;
}
void RwheelSpeed()
{
if(digitalRead(encoder1pinB))
Rduration--;
else Rduration++;
}
void control()
{
if(Serial.available()){
int val;
val=Serial.read();
switch(val){
case 'w':
if(move_flag<5)move_flag += 0.5;
else move_flag = 0;
break;
case 's':
if(move_flag<5)move_flag -= 0.5;
else move_flag = 0;
Serial.println("back");
break;
case 'a':
turn_flag = 1;
left_need = 0.6;
Serial.println("zuo");
break;
case 'd':
turn_flag = 2;
right_need = 0.6;
Serial.println("you");
break;
case 't':
move_flag=0;
turn_flag=0;
right_need = left_need = 0;
Serial.println("stop");
break;
default:
break;
}
}
}
void loop()
{
//control();
while (i2cRead(0x3B, i2cData, 14));
accX = ((i2cData[0] << 8) | i2cData[1]);
//accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
//gyroX = (i2cData[8] << 8) | i2cData[9];
gyroY = (i2cData[10] << 8) | i2cData[11];
//gyroZ = (i2cData[12] << 8) | i2cData[13];
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
double roll = atan2(accX, accZ) * RAD_TO_DEG - move_flag;
//double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = -gyroY / 131.0; // Convert to deg/s
//gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
//gyroYangle += gyroYrate * dt;
Kalman_Filter(roll,gyroYrate);
if(abs(angle)<35){
//Serial.println(angle);
pwm_calculate();
PWMD();
}
else{
analogWrite(ENB, 0); //PWM调速a==0-255
analogWrite(ENA, 0); //PWM调速a==0-255
}
delay(2);
}

复制代码

器件：Arduino UNO或者Mega2560、LM298模块，MPU6050传感器、两个编码电机，
公母杜邦线，蓝牙HT-06、锂电池18650（4）——（成本大概200左右）

如果用手机控制可以下载应用宝里面的一个“小蜜蜂机器人”APP，个人觉得这个APP功能超强大，大神写的APP就是屌！专门连接蓝牙的串口助手，还有调试舵机的功能，简直可以上天了。

调试过程中会学习到很多东西，希望大家可以看看，虽然我也做的很好，不过在这期间学习到了很多东西……
几天就可以看到你想看到的现象哟，还等什么，开始动手吧，慢慢看资料慢慢理解（不对之处，敬请谅解）
——初学者心声
http://v.youku.com/v_show/id_XMTUyODUwMDkxMg==.html

ID:1 · 发表于 2016-4-9 12:27

谢谢分享楼主能传个压缩包吗好像还有些头文件没有

ID:100312 · 发表于 2016-4-9 16:55

附件里面有wire的库文件，MPU6050原始数据分析，以及PWM输出的原理，还有小蜜蜂蓝牙助手的调试APP……

ID:91641 · 发表于 2016-4-15 22:35

s414545584 发表于 2016-4-9 16:55
附件里面有wire的库文件，MPU6050原始数据分析，以及PWM输出的原理，还有小蜜蜂蓝牙助手的调试APP……

arduino平衡车自动跑起来的时候怎么加上速度环呢？

ID:100312 · 发表于 2016-4-16 20:07

xiaobolinux 发表于 2016-4-15 22:35
arduino平衡车自动跑起来的时候怎么加上速度环呢？

直接加呀，代码里面有

ID:91641 · 发表于 2016-4-17 00:00

s414545584 发表于 2016-4-16 20:07
直接加呀，代码里面有

但是我的平衡车能平衡但是只是往一个方向跑，跑一段就倒了，不能原地控制。

ID:91641 · 发表于 2016-4-17 09:54

s414545584 发表于 2016-4-16 20:07
直接加呀，代码里面有

哪句是关于速度环的呀

ID:91641 · 发表于 2016-4-17 10:23

然后可能会出现倒，或者自动跑起来的时候加上速度环
//这时就会很稳定的站起来，然后用小力气的手推不会倒。

这个不行啊，一直往前，倒。然后代码里面没有加速度环呀，，怎么加呀，，速度环是啥呀，，求教

ID:100312 · 发表于 2016-4-17 12:21

xiaobolinux 发表于 2016-4-17 10:23
然后可能会出现倒，或者自动跑起来的时候加上速度环
//这时就会很稳定的站起来，然后用小力气的手推不会倒 ...

加q414545584，私聊，交流一下

ID:134558 · 发表于 2016-7-21 23:21

你好请问在哪儿可以调初始角？

ID:134558 · 发表于 2016-7-22 19:49

请问一下楼主那个电机编码器的线接到车上的脚是要怎么定义？

ID:100312 · 发表于 2016-7-23 12:21

yangtao 发表于 2016-7-21 23:21
你好请问在哪儿可以调初始角？

double roll = atan2(accX, accZ) * RAD_TO_DEG - move_flag;
这个就是x轴方向的角度，move_flag就是偏移量，调偏移量，用串口读数直到基本为0

ID:100312 · 发表于 2016-7-23 12:22

yangtao 发表于 2016-7-22 19:49
请问一下楼主那个电机编码器的线接到车上的脚是要怎么定义？

编码电机上的AB相线一个用来检测高低电平然后自加或者自减，另外一个用来触发中断函数，你可以看上面的引脚定义就知道啦

ID:134558 · 发表于 2016-7-23 23:34

s414545584 发表于 2016-7-23 12:22
编码电机上的AB相线一个用来检测高低电平然后自加或者自减，另外一个用来触发中断函数，你可以看上面的引 ...

谢谢了已经调平衡了

ID:116662 · 发表于 2016-8-2 14:11

好资料，辛苦楼主了

ID:137339 · 发表于 2016-8-22 22:46

赞一个

ID:137396 · 发表于 2016-8-23 15:28

這個還ˊ需要再修改一下使可以適應四處移動

ID:100312 · 发表于 2016-8-25 12:21

a98765432180 发表于 2016-8-23 15:28
這個還ˊ需要再修改一下使可以適應四處移動

里面不是有个control函数吗，那就是用蓝牙控制的呀……

ID:139027 · 发表于 2016-9-10 19:32

楼主可以分享个接线图吗？

ID:189412 · 发表于 2017-4-14 06:36

thank you so much.

ID:173821 · 发表于 2017-4-17 03:09

本帖最后由 bonzewu1213 于 2017-4-19 03:16 编辑

請問樓主，目前我按您貼的源碼，上傳後，僅有右輪正常運轉，傾斜加速，反向傾斜，反向加速，但僅有右輪會動，左輪完全不動，是不是源碼有地方被註記起來，沒有打開，謝謝。

ID:173821 · 发表于 2017-4-17 20:54

哈哈，ENA及ENB應接11，12，接錯了，接成10，11，改正後就可以了，已經解決，只是重心還要調一下，基本上已正常運作，太棒了，感謝樓主，大大感謝。

ID:191204 · 发表于 2017-4-19 14:07

bonzewu1213 发表于 2017-4-17 03:09
請問樓主，目前我按您貼的源碼，上傳後，僅有右輪正常運轉，傾斜加速，反向傾斜，反向加速，但僅有右輪會動 ...

我上传后怎么两个轮子都没动

ID:173821 · 发表于 2017-4-20 00:36

仔仔12 发表于 2017-4-19 14:07
我上传后怎么两个轮子都没动

據我所知，L298N 有分A,B 兩型, 你要買那種 6根排針上面都沒有jumper 的那種，馬達電源需要有12V，12V接線要接對，接錯，很容易把線燒著了，得趕緊滅火 XD 如果接線都對了，應該跑得起來才對，原始作者，應該給他大大讚一個！

ID:216395 · 发表于 2017-7-2 17:55

不错，真的不错，我正在找呢

ID:216395 · 发表于 2017-7-2 17:56

这些代码我正需要，没想到在这个网站找到了，不错！

ID:232830 · 发表于 2017-9-13 10:54

这些代码我正需要，没想到在这个网站找到了，不错！

ID:232830 · 发表于 2017-9-13 10:54

这些代码我正需要，没想到在这个网站找到了，不错！

ID:214383 · 发表于 2017-9-16 00:50

学习了

ID:238956 · 发表于 2017-10-12 19:40

学习一下

ID:177577 · 发表于 2017-11-27 19:54

楼主有接线图吗？

ID:254763 · 发表于 2017-11-28 16:40

好棒，不知运行结果如何。

ID:233519 · 发表于 2018-5-1 10:04

这个厉害了！！！

ID:315589 · 发表于 2018-5-3 11:21

真的很溜哦

ID:79544 · 发表于 2018-7-29 18:10

楼主您好！分享出原理图啊谢谢！！

ID:161201 · 发表于 2019-1-15 02:28

好棒，不知运行结果如何

ID:540025 · 发表于 2019-5-16 17:17

感谢楼主的分享，刚从百度上搜到这篇文章，小蜜蜂app的大神真棒。就是app资源确实难找

帐号		自动登录	找回密码
密码			立即注册

Arduino自平衡小车……你值得拥有

评分

相关帖子

评分

评分