本帖最后由 生来孤独 于 2014-12-11 17:32 编辑
http://v.youku.com/v_show/id_XODQ2NTIzNTky.htmlhttp://v.youku.com/v_show/id_XODQ2NDgzMjEy.htmlhttp://v.youku.com/v_show/id_XODQ2NDgwNTYw.html前段时间DIY了一辆两轮平衡小车，感觉挺有意思。很多资料都是参考的其他大神的，本着受益于开源，回馈于开源的精神。现在也把自己的做品拿出来晒晒！
故事很长，不想听的建议倒着看~~~
//********************************************************************************************************//
大二的时候就想做一辆平衡小车玩，那时刚刚学习STM32基本上编程也是瞎搞的，很多都是直接copy 瞎改，效果可想而知 ....... 后来也不了了之放弃了
附当时图片两张：
 
之后就没有再考虑平衡车，知道自己渣，做不出来~~~~~
转眼暑假，没有回家，留在学校里参加暑假的大学生电子科技比赛（TI杯），那时选择了个寻找铁疙瘩的题目~~~ 说起来比较简单了呐~~~就**的个传感器，跟循迹一样的原理但没做好~~~最痛苦的是接线，用了好多的杜邦线，乱七八糟的几十根有的~~
搞到 最后看到电线头都大了~~~
再来一张~~
暑假过后自己也没有回家，然后就在寝室里上上网，看看书，打打游戏，出去打打篮球等~~
最重要的是学习绘制电路板，，，，，开始安装了protle99，，，越用月感觉不对劲儿~~后查了查安装了Altium Designer14 之后就是宅寝室浑浑噩噩的看视频，照着比划了~~~~
当时自评价自己的一句话就是 忙忙碌碌却又碌碌无为~~~~（一个人挺伤感的...）
俗话功夫不负有心人自己画了自己的第一块板子---------STM32F103 64引脚的最小系统板
 
下面进入正题
先展示一张成品图吧：
 
主控芯片选用 STM32f103RBT6
电机驱动选用 东芝TB6612
传感器模块选择 MPU6050
蓝牙模块 （能连上就行）
电源供电 12V——（7805）——5V——（1117）——3.3V
然后就是设计板子外形尺寸然后布线
上面是控制板，还有一块电源板子，用于把12V电源稳压到5V 然后输送过来，另一方面为了给小车起到支撑作用，图就不上了,最后我会把工程上传上来的。
然后是还能接电路，为了焊好这东西自己也是蛮拼的，之前先买了一些焊接贴片元件的板子，用来练手。之后为了焊接QFN封装的芯片，还买风枪和一些QFN封装的芯片的模块，拆了焊焊了拆的~~~终究也是练到了好的技术
晒晒自己手工焊好的板子吧！！！！
哇哈哈，感觉很有成就感啊~~~~
再说一下电机和电池（排除广告嫌疑~~~~）
下面开始扯一下程序算法~！！！！！！
控制算法：
1.倾角环 得到t时刻的小车平面 和 地理水平面的夹角（正负偏差） + 此夹角 的变化速度 2.车速环 得到小车整体的运动速度 得到小车的位移量
3.PWM输出 左右电机通过PWM驱动TB6612控制输出
定义变量： Angle_err 小车平面和地理平面的角度偏差 Angle_Gyro 小车角度变化的速度 （角度偏差的变化速度【Gyro 陀螺仪】） Moto_Speed_L 小车左边电机的速度 Moto_Speed_R 小车右边电机的速度 Moto_Disp_L 小车左边电机的位移 Moto_Disp_R 小车右边电机的位移
核心计算公式： PWM_L = (Kp_1 * Angle_err + Kd_1 * Angle_Gyro) + (Kp_2 * Moto_Disp_L + Kd_2 *Moto_Disp_L)
PWM_R = (Kp_1 * Angle_err + Kd_1 * Angle_Gyro) + (Kp_2 * Moto_Disp_R + Kd_2 *Moto_Disp_R)
核心代码注释： void IMU_GetAngle_Now_(float *Ang) {
float Angle_1; float Angle_2; short acc[3]; 存放加速度数据 0,1,2， 对应 XYZ轴 short gyro[3]; 存放陀螺仪数据 0,1,2 对应 XYZ轴
MPU6050ReadAcc(acc); 读取传感器的XYZ轴的加速度分量AD 值 0~2`16 Angle_1 = atan((acc[0]*1.0f) / (acc[2]*1.0f)); Angle_1 计算当前的角度值 反正切 Angle_1 = (Angle_1 * 180 ) / 3.14 ; 反三角函数计算得到的是弧度制，抓换为 角度值 MPU6050ReadGyro(gyro); 得到传感器的角速度 （通过陀螺仪）
Gyro += ( -gyro[1] / 16.4f )*0.02 ; AD值 转换为角度值 计算原理参见传 感器数据手册 转换为角度值后进行积分处理0.02为积 分采样时间20ms
Angle_2 = 0.98 * Gyro + 0.02 * Angle_1; 角度的低通滤波，以消除震动和干扰误差
Gyro = Angle_2 ; 防止陀螺仪误差积分漂移 Ang[0] = Angle_2+3.18; Ang【0】 当前偏差角度 + 3.18 传感器 安装偏差（零偏） Ang[1] = (-gyro[1] / 16.4f) * 0.01; 纠正陀螺仪方向 返回角速度 } 滤波效果图：（说真的，没搞懂卡尔曼滤波~~~~~这个效果也不差，还简单）
绿色 加速度计算角度 黄色 低通输出曲线 红色 陀螺仪跟踪曲线
输入参数 当前角度 角速度 输出参数 void void IMU_Control_Gyro4(float Angle ,float Gyro) { static int speed[9] ; int Left , Reght ;
if(Angle > -45 && Angle < 45) 当前角度情况判断 { //得到 速度 和 位移 //speed[0] speed_L 左边速度 //speed[1] speed_R 右边速度 //speed[2] //speed[3] //speed[4] speed_move_L 位移L //speed[5] speed_move_R MOTO_Get_Speed2(speed); //直立 Left = Angle * 75 + Gyro * 75 ; Reght = Angle * 75 + Gyro * 75 ; //叠加速度位移控制 speed[7] = speed[4] * 2.0 + speed[0] * 28 ; speed[8] = speed[5] * 2.0 + speed[1] * 28 ; //对叠加的速度位移控制做平滑输出处理 speed[7] *= 0.09 ; speed[8] *= 0.09 ; //直立控制和平滑后的速度位移控制相叠加 Left += speed[7] ; Reght += speed[8] ; //PWM通过电机驱动模块输出到电机 TIM3_Pwm_Set(Left,Reght); } else { NC = 0 ; 关闭电机 LED1 = !NC ; LED关闭 } }
void MOTO_Get_Speed2 (int *V) { int I_ = 1000 ; //得到计数器值 V[0] = TIM_GetCounter(TIM2); //A0 V[1] = TIM_GetCounter(TIM1); //A12 //计数器清零，准备下一次采样 TIM_SetCounter( TIM1,0); / TIM_SetCounter( TIM2,0); //由电机当前的驱动方向，判断电机转动方向的正负，进而判断速度的正负 if(AIN_1==1) V[0] = -V[0] ; if(BIN_1==1) V[1] = -V[1] ; //对速度做平滑处理，类似之前的角度处理方法 V[0] = V[2] * 0.7 + V[0] * 0.3 ; V[1] = V[3] * 0.7 + V[1] * 0.3 ;
V[2] = V[0] ; V[3] = V[1] ; //左右车轮取平均值输出 使其做直线运动 V【4】当前的车速 V[0] = ( V[0] + V[1] ) / 2 ; V[1] = V[0] ; //速度累加 的到位移 左右车轮的位移 V[4] += V[0] ; V[5] += V[1] ; //对位移（积分）的饱和处理 防止误差积分偏移过远 造成速度位移控制的不稳定 if( V[4] < -I_) V[4] = -I_ ; if( V[4] > I_ ) V[4] = I_ ; if( V[5] < -I_) V[5] = -I_ ; if( V[5] > I_ ) V[5] = I_ ; //增加小车的位移控制量 前进 后退 左转 右转 V[4] = V[4] + MOTO_SPEED_L ; V[5] = V[5] + MOTO_SPEED_R ; }
void TIM3_Pwm_Set(int LL,int RR) { ************************************************** if(RR<0) AIN_1=1,AIN_2=0; else AIN_1=0,AIN_2=1;
if(LL<0) BIN_1=1,BIN_2=0; else BIN_1=0,BIN_2=1; ************************************************** LL = abs(LL)+25; 增加控制死区 RR = abs(RR)+25;
if(LL>=999) LL=999; 控制饱和处理 if(RR>=999) RR =999;
TIM_SetCompare2(TIM3,LL); 电机对应的PWM通道输出 TIM_SetCompare1(TIM3,RR); }
中断判断标志位：
if(TIM4_IMU) { IMU_GetAngle_Now_(IMU); IMU_Control_Gyro4(IMU[0],IMU[1]); TIM4_IMU=0; }
最后再上一张示意图，，，，，有点抽象，自己画的
嗯，，，，，，，，，该说的都说完了~~~ 上电，烧程序，小车跑起来了~~~
下面2楼上传视频和工程代码.........（不要和我抢！！！！！）
关于遥控，自己只做了按键的控制，没有再做6050的姿态控制，本打算通关按键切换控制模式，既可以按键控制也可以6050姿态控制（类似空中鼠标把），后来事情有点多，拖一拖，回头再弄，大体上已经完成！！！！小车一些参数可能还要再细细调一调!!!!!
画了多少钱？？？？
八九百吧~~~~~乱七八糟的，打板子画了400大样，有很多设计废的~~~~ 买芯片什么的，差不多就是这个数！！！不过感觉还是很值得的！！！远比要买一个现成的划算得多！！
写的哼乱啊啊~~~
资料在七楼和十楼！！！！
| 
[复制链接]
发表于 2014-12-11 15:27:18
楼主
我再看看
