极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 78744|回复: 32

Arduino平衡小车——开始不断更新

[复制链接]
发表于 2017-6-4 17:32:28 | 显示全部楼层 |阅读模式
本帖最后由 小学徒 于 2018-1-3 01:30 编辑

考试繁忙,无暇细细道来。。。
先上一波片



做平衡需要具备的基本配件:
一.硬件方面:
1.电源:比如航模电池11V或者7V以上,具体看电机供电电压

2.Arduino推荐2560,因为后面可以加很多有趣的东西,比如语音,无线遥控,超声波,触摸屏,显示器等等,MEGA2560有足够的引脚
3.关键的东西>>MPU6050

4.电机和轮子+车架(可自制)。建议初学者买带有编码测速的车身,电机应带有两相测速,目的是可以测出电机正反转。

5.电机驱动:可用298或者TB6612.建议用TB6612,它体积小,功率够用。

6.降压模块。降压模块主要给Arduino供电,把11V的电压降到8V左右通过Vin供电端给Arduino供电,如果直接接11V,小心芯片Duang机。
小车可平衡之后就可以加点控制器件:比如HC-05/06蓝牙模块或者NRF2.4G无线模块。
二.软件方面:
1.最重要的当然是传说中的PID控制算法(其实不难,真不难,一个学期时不时的拿别人的程序出来看看,总会有理解的那天!)
2.理解测速和检测方向原理(不管最高最低转速如何,只要转动电机有数就好,不用担心数太小)
3.灵活运用串口监视器进行调试,可以打印数字观察角度,PWM等。
4.要知道MEGA2560和UNO的定时器库不一样,需注意!
#include <FlexiTimer2.h>//mega2560定时器库
#include <MsTimer2.h>//如果MsTi该库不变红色,需更新该库(UNO定时库)
5.拿到正确的MPU6050的角度和角速度。为什么说拿到呢?实话说6050不需要你太花费时间去理解它是怎么工作怎么叽叽歪歪的,你只需要找到它的出口值(角度和角速度)就行。就比如下面这段程序就可以得出标准的角度(f_angle )和角速度(omega)
当然在主函数钱还有它的一些定义:如何在一大堆程序里找出哪些才和它相关呢(先整体复制进来-编译运行-正确-尝试屏蔽掉自认为与它无关的语句以及定义-若报错,则它有关,恢复回来-若正常编译,则可去掉该语句)
/********MP6050相关***************/
float timeChange=10;//滤波法采样时间间隔毫秒
float dt=timeChange*0.001;//注意:dt的取值为滤波器采样时间
MPU6050 Mpu6050;//命名 陀螺仪类:Mpu6050
int16_t ax, ay, az, gx, gy, gz;//陀螺仪原始数据 3个加速度+3个角速度
float f_angle,angleA,omega,gyroGz,control_v;
float angle0 = 1.80,gy1 = 30;        //因重心导致角度偏置;陀螺仪偏移量

/**************标准角度处理**************/
void jiao_du()
{
        Mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取原始6个数据
        float ax1 = map(ax,-15924,16629,-16384,16384);     //除去每个轴的漂移量
        float ay1 = map(ay,-16601,16121,-16384,16384);
        float az1 = map(az,-17454,15750,-16384,16384);
        angleA = atan2(ax1,az1) * 180 / PI+ angle0; //加速度计算角度。得到平衡位置的角度
        omega = -(gy - gy1) / 131.00;   //计算角速度
        gyroGz =gz / 131.00;//陀螺仪角速度,注意正负号与放置有关
        unsigned long now = millis();        // 当前时间(ms)
        float dt = (now - preTime) / 1000.0; // 微分时间(ms)
        preTime = now;  
        float K = 0.8;  //一阶滤波
        float A = K / (K + dt);                     
        f_angle = A * (f_angle + omega * dt) + (1-A) * angleA; //得到正常的角度
        }
6.(不需要)卡尔曼滤波。其实用了上面的角度处理程序,就不要卡尔曼了
因为要得到正常的角度和角速度有两种方式:
1.算法融合得到(简单)
2.卡尔曼滤波得到(复杂)
总接线图:

电机后背实物图:(此图来自平衡小车之家)

三.万事俱备只欠东风
1.第一步,测试电机控制。
        根据上面的接线图,把电机,驱动,电源,Arduino接好,然后用下面这段程序测试,电机是否能转,转的方向对不对,若出现不正常则检查相关信号线,比如转向反了(只需在程序里把IN对调或者把接线调即可)
电机测试程序:
//LM298
#define EA 5//左  UNO的PWM引脚为3、5、6、9、10、11
#define EB 6//右

#define IN1 11  //左轮
#define IN2 12
#define IN3 9   //右轮
#define IN4 10
void setup(){
      pinMode(EA,OUTPUT);
      pinMode(EB,OUTPUT);
      pinMode(IN1,OUTPUT);
      pinMode(IN2,OUTPUT);
      pinMode(IN3,OUTPUT);
      pinMode(IN4,OUTPUT);
                  }
void loop()
{
  PWM_Motor(200,-200);//测试电机,应看到左轮前进,右轮后退
}
/***********电机驱动子程序*************/
void PWM_Motor(int PWM_L,int PWM_R)
{
  if(PWM_L<0)
  {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  }
  else
  {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
   }
  if(PWM_R<0)
  {
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
  }
  else
  {
   digitalWrite(IN3, LOW);
   digitalWrite(IN4, HIGH);
  }

analogWrite(EA,abs(PWM_L));//5脚
analogWrite(EB,abs(PWM_R));//6脚
}
不知道大家有没有注意到:在这里判断正反转是通过判断PWM的正负来确定正反转的,而不是用角度来判断,
比如
错误的方向判断方式:
if(角度<0) {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  }
else......
正确的方向判断方式:
if(PWM_L<0) {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  }
  else
  {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
   }

/****************************************可平衡代码讲解*******************************************************/

#include <Wire.h>
/***********基于MEGA2560*******/
//#include <MsTimer2.h>//如果MsTi该库不变红色,需更新该库(UNO定时库)
#include <FlexiTimer2.h>//mega2560定时器库
#include "I2Cdev.h"
#include "MPU6050.h"

/******无线库*******/
#include "SPI.h"                           
#include "Mirf.h"
#include "nRF24L01.h"
#include "MirfHardwareSpiDriver.h"

//LM298
#define EA 5//左  UNO的PWM引脚为3、5、6、9、10、11
#define EB 6//右

#define IN1 11  //左轮
#define IN2 12
#define IN3 9   //右轮
#define IN4 10

const int encoderPinA = 2;//中断0
const int encoderPinB = 3;//中断1
/********MP6050相关***************/
float timeChange=10;//滤波法采样时间间隔毫秒
float dt=timeChange*0.001;//注意:dt的取值为滤波器采样时间
MPU6050 accelgyro;//陀螺仪类
int16_t ax, ay, az, gx, gy, gz;//陀螺仪原始数据 3个加速度+3个角速度
float f_angle,angleA,omega,gyroGz,control_v;
float angle0 = 1.80,gy1 = 30;        //因重心导致角度偏置;陀螺仪偏移量
//******卡尔曼参数************
static const double C_0 = 1;
static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dtt=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;
float bias;//调整机械中值后的最终角度
/******时间参数****************/
unsigned long now;
unsigned long preTime = 0;
unsigned long time = 0;
unsigned long lastTime;
unsigned long timespl = 0, timespr = 0; // 测速时间标记
//******电机参数************
int speed_mL ; //左电机转速片
int speed_mR ; //右电机转速片
float Speed;
float Position; //位移
float speedcar; //左右轮平均速度


float Pwm_angle;   //    直立环节
float Pwm_speed;   //    速度位移环节
float   Pwm;       //最终PWM计算

int   PWM_RR;         //右轮PWM值计算
int   PWM_LL;         //左轮PWM值计算
//******遥控参数*************
char  val;          //接收参数
int  turn_need=0;//转向参数
int  speed_need=0; //遥控速度参数
char Tata;//无线存放地址
void Mirfinit()//无线初始化
{
//无线初始化参数
  Mirf.spi = &MirfHardwareSpi;         
  Mirf.init();
  Mirf.setRADDR((byte *)"fasoG"); //五个标识符
  Mirf.payload = sizeof(unsigned char);  //一次性传送的数据量大小
  Mirf.channel = 3;//通道
  Mirf.config();
}

void setup() {
   
      Wire.begin();
      Serial.begin(9600);//电脑串口通信
      accelgyro.initialize();//MPU6050初始化。
      Mirfinit();//无线初始化
      pinMode(EA,OUTPUT);//L298
      pinMode(EB,OUTPUT);
      pinMode(IN1,OUTPUT);
      pinMode(IN2,OUTPUT);
      pinMode(IN3,OUTPUT);
      pinMode(IN4,OUTPUT);
     
      /********中断 测速用***************/
      pinMode(2,INPUT_PULLUP);//中断输入引脚
      pinMode(3,INPUT_PULLUP);//中断输入引脚
  //测速出来两根线,一根接中断引脚,另一根接任意脚,目的是读取高低电平进而判断方向,在中断时该加加或减减   
      pinMode(4,INPUT);// 左方向判断电机转向正负
      pinMode(23,INPUT);//右方向
  //定时中断,可以在定时中断服务程序里放读取角度、PWM计算等运行代码
  //必要时,应在中断里面“分批计时”。比如多少周期才运行角度读取,经过多久才进行pid计算,,,
  //loop则不用放太多东西   
  FlexiTimer2::set(15, flash); // 15ms,要执行的子函数
  FlexiTimer2::start();
  attachInterrupt(0, stateL, FALLING );//中断0==2脚
  attachInterrupt(1, stateR, FALLING );//中断1==3脚

}
/*
* LOOP里面就放这么多!!!其余放中断。
*/
void loop()
{
  //PWM_Motor(255,-255);//测试电机,应看到左轮前进,右轮后退//具体看你的L298接线顺序
  WXsw();//无线遥控
  da_yin();//串口打印子程序Serial.print//调试用
}

//应养成打包做成子程序的习惯,方便调用,也方便调试!
//8月31晚更新!.快要断网了,暂放这么多。
        网站没有实时效果显示,保存才发现格式乱了,看起来有点头疼。希望论坛里的老司机多传授点发帖的格式整理经验。。。
编程过程,可以参考一下



MPU6050视频里写的程序:
/*做平衡车需要能正确读出MP6050的角度和角速度
* 本程序即为 角度和角速度的示范处理
* 方式一:不通过卡尔曼
* 方式二:待写
* 按理说:新手不用太纠结于 卡尔曼之类的东西,你只需要懂得如何取出角度和角速度即可。
*/
//MPU6050需要的库  
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
//相关参数定义:不用理会。直接从其他地方复制过来,看效果。。
float timeChange=10;//滤波法采样时间间隔毫秒
float dt=timeChange*0.001;//注意:dt的取值为滤波器采样时间
MPU6050 accelgyro;//陀螺仪类
int16_t ax, ay, az, gx, gy, gz;//陀螺仪原始数据 3个加速度+3个角速度

float kp = 30,kd = 1.8,ksp = 0.006 ,ksi = 1.2; //例如这一句是其它地方用到的,在这可以屏蔽

  // Pwm = kp*angle + kd*omega + kp*speed +ki*position(角度,角速度,反馈速度,位置)
float f_angle,angleA,omega,gyroGz;
float angle0 = 1.78,gy1 = 30;        //因重心导致角度偏置;陀螺仪偏移量
unsigned long now;
unsigned long preTime = 0;
//角度的读取 直接复制
void jiaodu()
{
        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取原始6个数据
        float ax1 = map(ax,-15924,16629,-16384,16384);    //除去每个轴的漂移量
        float ay1 = map(ay,-16601,16121,-16384,16384);
        float az1 = map(az,-17454,15750,-16384,16384);
        angleA = atan2(ax1,az1) * 180 / PI+ angle0;     //得到平衡位置的角度
        omega = - (gy - gy1) / 131.00;//计算角速度( - 30)
        gyroGz = gz / 131.00;
        unsigned long now = millis();                           // 当前时间(ms)
        float dt = (now - preTime) / 1000.0;                    // 微分时间(ms)
        preTime = now;  
        float K = 0.8;                    //一阶滤波
        float A = K / (K + dt);                     
        f_angle = A * (f_angle + omega * dt) + (1-A) * angleA;      //得到正常的角度
        //正常角度:f_angle
        //角速度:  omega
        //平衡车里就用到上面两个输出值
        //例如:jiao_du_PWM =f_angle*KP  + omega * KD; //角度乘于KP系数  加上  角速度乘于KD参数(微分);
}
void setup() {
  //初始化一些东西
  Wire.begin();
  Serial.begin(9600);//打开串口
  accelgyro.initialize();//MPU6050初始化。  从示例文件复制过来

}

void loop() {
  //运行角度处理子程序
  jiaodu();
  //1.打印角度,检验程序是否正确或者能否读出角度
  //Serial.print("jiaodu= ");
  //Serial.print(f_angle);
  //角度输出正常(逆时针为负,顺时针为正)
//2.打印角速度
  //Serial.print("  jiaosudu= ");
  //Serial.println(omega);
//3.通过绘图观察 角速度的方向:可以观察到当瞬间逆时针摆动时 方向为正,瞬间顺时针摆动则为负。若反向则只需要
//在omega前加-即可  例如:jiao_du_PWM =f_angle*KP  - omega * KD;
Serial.print(" ");
  Serial.println(omega);
  

}
2017/13,29下午更新
        大家提的问题会出现在这里:
问题一:啥是微分先行?
回答:@  例如平衡车里的角速度就相当是微分,当小车有大的角加速度(你用手猛拨动它一下下),出现一点倾斜时,小车就会依据"微分-角速度"在那一瞬间做出反应,电机迅速向要倒的方向转动,目的是防止小车进一步倒下去。这就是微分先行(不是快要倒地了才作用,而是有倒地的一点趋势就要先行作用)

本帖子中包含更多资源

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

x
回复

使用道具 举报

发表于 2017-6-11 20:02:41 | 显示全部楼层
666,是平衡小车群的小学徒么,功能又有增强不少啊
回复 支持 反对

使用道具 举报

发表于 2017-6-11 21:43:49 | 显示全部楼层
手头有材料,也准备做一个,怎奈是个小白。求大神分享点资料吧!邮箱[email protected]
回复 支持 反对

使用道具 举报

 楼主| 发表于 2017-6-12 10:59:39 | 显示全部楼层
Dreamer-Men 发表于 2017-6-11 20:02
666,是平衡小车群的小学徒么,功能又有增强不少啊

是的  还有很多新鲜功能等待开发
回复 支持 反对

使用道具 举报

 楼主| 发表于 2017-6-12 11:02:40 | 显示全部楼层
shh80s 发表于 2017-6-11 21:43
手头有材料,也准备做一个,怎奈是个小白。求大神分享点资料吧!邮箱

是小白的话  建议先挣扎个半把月
回复 支持 反对

使用道具 举报

 楼主| 发表于 2017-6-12 11:06:12 | 显示全部楼层

资料只有等到有时间整理了才能发出
回复 支持 反对

使用道具 举报

发表于 2017-6-12 21:29:21 | 显示全部楼层
小学徒 发表于 2017-6-12 11:06
资料只有等到有时间整理了才能发出

期待资料早日整理出来,我的器件需要等待它光明的一天。
回复 支持 反对

使用道具 举报

发表于 2017-7-8 17:56:37 | 显示全部楼层
求大神分享点资料吧!邮箱[email protected]
回复 支持 反对

使用道具 举报

 楼主| 发表于 2017-8-19 22:46:42 | 显示全部楼层
接下来以网友的问题为更新方向,若没有则暂停更新。
回复 支持 反对

使用道具 举报

发表于 2017-8-22 11:47:07 | 显示全部楼层
请问lz怎么把AB相加入到速度环中
回复 支持 反对

使用道具 举报

发表于 2017-8-23 21:21:29 | 显示全部楼层
开始做平衡车半年多了,网上查过很多资料,也套用过不少代码,有些虽然能运行,但注释太少,对一个非专业爱好者来说,有些地方根本看不懂,只能生搬硬套代码,因此小车总是摇摇摆摆,没有站稳过……
楼主给的资料很详细,花了一下午时间,参照注释一句代码一句代码地读,居然读通了。期待楼主后续的教程!
回复 支持 反对

使用道具 举报

 楼主| 发表于 2017-8-24 01:51:55 | 显示全部楼层
monster.yang 发表于 2017-8-23 21:21
开始做平衡车半年多了,网上查过很多资料,也套用过不少代码,有些虽然能运行,但注释太少,对一个非专业爱 ...

哈哈 你这才给我继续更新的动力
回复 支持 反对

使用道具 举报

 楼主| 发表于 2017-8-24 02:04:57 | 显示全部楼层
星空为我而闪耀 发表于 2017-8-22 11:47
请问lz怎么把AB相加入到速度环中


首先你这个问题很模糊。
应该这样理解:
AB相:可以这样用,A相接2中断脚,B相接任意引脚,,比如接4.然后在中断服务程序里这样写:
/**********中断触发要运行的子函数**************/
void stateL()
{
  if(digitalRead(4) ==1)
  speed_mL++;//正转
  else
  speed_mL--;//或反转

}
速度环:
void Psn_Calcu()
{
  speedcar =(speed_mL+speed_mR)*0.5; //===获取最新速度偏差
  
  Speed_turn =speed_mL - speed_mR;//两边等速
  SPEED_TURN =Speed_turn;         //两边等速
  
  Speed *=0.8;         //===一阶低通滤波器
  Speed +=speedcar*0.2;//===一阶低通滤波器  
  Position+=Speed;//===积分出位移 积分时间:5ms位移cm
  //Position = Position - speed_need;//蓝牙控制相关
  Position +=speed_need;
  if(Position>1000)  {Position=1000;}//===积分限幅
  if(Position<-1000) {Position=-1000;}

}
/*******************************************速度环调试*************************************************/
//Pwm=vKP*Speed;//速度调试
//Pwm=vKI*Position;//位移调试
回复 支持 反对

使用道具 举报

发表于 2017-8-24 13:27:19 | 显示全部楼层
小学徒 发表于 2017-8-24 02:04
首先你这个问题很模糊。
应该这样理解:
AB相:可以这样用,A相接2中断脚,B相接任意引脚,,比如接4 ...

有点不是很理解lz的代码,粗略看了一下,其实lz只用到了单相,那还有一相的作用是什么,我在网上找到的通常使用四倍频的方法来做,目的很明显,为了更好的控制电机,虽然感觉不是很懂,嗯。。还有就是通过B相来判断转向,,还有一个地方,lz的代码中应该是想同步转速吧,但是通过转速积出位移是什么意思,还有那句两边等速的代码有什么用,最后的速度环调试应该是想通过对转速比例或者对位移积分来达到控制转速的结果吧,但我印象中的PID的I是不能单独使用的,麻烦楼主解释一下疑问哈
回复 支持 反对

使用道具 举报

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

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

Archiver|联系我们|极客工坊

GMT+8, 2024-3-28 17:06 , Processed in 0.058686 second(s), 24 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

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