拜坊友所赐，我的平衡车制作成功啦！
本帖最后由 jkhjt1949 于 2016-2-19 12:09 编辑我是看到论坛里的视频后才萌生DIY平衡小车的。本人大二党，花了2个月才做到这个程度的，完全出于兴趣，拿出来就是想跟大家交流交流。不好意思，文件和图片上传好多次都不成功。在末尾我留了网盘链接了，里面有材料清单和小车资料，有需要的可以下载。欢迎交流！！！
为了偷懒，视频是用4s拍的，效果一般，屏幕较窄，请见谅。
此小车因为安装了蓝牙模块，可用android手机操控,完成前进后退，左右方向旋转动作
http://v.youku.com/v_show/id_XNzgzNTcyNzMy.html
详细图片
#include <I2Cdev.h>
#include <MPU6050.h>
#include<Kalman.h>
#include <Wire.h>
MPU6050 accelgyro;
Kalman kalmanFilter;
#define IN1 5
#define IN2 6
#define IN3 9
#define IN4 10
#define ENA 7
#define ENB 8
#define POSITION_RATIO 0.0572
int16_t ax,ay,az;
int16_t gx,gy,gz;
int data,lastData;
long count,count2;
double aax,aay,aaz,ggx,ggy,ggz, nowTime, lastTime ;
float accAngleY,kalAngleY, Ax,Ay,Az, _position,lastPosition,positionR,positionL,positionDf ;
floatpwm, pwmL, pwmR, dt, _speed,lastSpeed,acceleration;
floatdrift,setPoint,initialSp,turnSpeed, itermIn, k,k2,k3,kp,ki,kd,kps,ksp;
void setup()
{
Wire.begin();
Serial.begin(9600);
accelgyro.initialize();
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
pinMode(IN3,OUTPUT);
pinMode(IN4,OUTPUT);
pinMode(12,INPUT);
pinMode(11,INPUT);
digitalWrite(IN1,0);
digitalWrite(IN2,0);
digitalWrite(IN3,0);
digitalWrite(IN4,0);
attachInterrupt(0, encoder,FALLING);
attachInterrupt(1,encoder2,FALLING);
stopCar();
drift = 20 ;
k = 0.0 ;
k2 = 0.0 ;
//k3 = 0.0 ;
setPoint = 0.0 ;
turnSpeed = 0.0 ;
kp = 36.0 ;
ki = 0.0 ;
kd = 1.88 ;
kps = 0.145 ;
ksp = 1.36 ;
}
void loop()
{
blueTooth();
loopPeriod();
angleMeasure();
speedAndPositionCal();
//setPointAdj();
pwmCalAndOut();
delay(10);
}
//------------------------------------------------------------------------------------------
void loopPeriod()
{
nowTime = millis();
dt = (nowTime-lastTime)/1000;
lastTime = nowTime;
}
//------------------------------------------------------------------------------------------
void angleMeasure()
{
accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
Ax=ax/16384.00;
Ay=ay/16384.00;
Az=az/16384.00;
ggx=gx/131.00;
accAngleY=atan(Ay/sqrt(Ax*Ax+Az*Az))*180/3.14;
kalAngleY = kalmanFilter.getAngle( accAngleY, ggx, dt );
}
//------------------------------------------------------------------
void speedAndPositionCal()
{
positionR =count* POSITION_RATIO ; // unit : mm
positionL =count2* POSITION_RATIO ;
positionDf = positionR - positionL;
_position =( positionR + positionR ) / 2;
_speed = ( _position - lastPosition ) / dt ; //unit : mm/s
acceleration = ( _speed - lastSpeed ) / dt;
lastPosition = _position;
lastSpeed = _speed ;
}
//---------------------------------------------------------------------------------------------------------
void pwmCalAndOut()
{
if( kalAngleY < -drift || kalAngleY > drift) //角度过大，关闭电机
{
stopCar();
return;
}
itermIn += ( kalAngleY - setPoint ) ;
if( itermIn < -500 ) itermIn = -500;
if ( itermIn > 500 )itermIn = 500;
if( (_position > 100) || (_position < -100) )
{
count = 0;
count2 = 0;
lastPosition = 0;
}
pwm= kp * ( kalAngleY - setPoint )+ ki * itermIn + kd* ggx + kps * _position + ksp * _speed + k2 * acceleration;
pwmL = pwm+ turnSpeed + positionDf * k;
pwmR = pwm- turnSpeed - positionDf * k;
if( pwmL < 0 )
{
pwmL = -( pwmL - 0 ) ;
if( pwmL >255 ) pwmL = 255;
analogWrite(IN1,0);
analogWrite(IN2,pwmL);
}
else
{
pwmL += 0;
if( pwmL >255 ) pwmL = 255;
analogWrite(IN2,0);
analogWrite(IN1,pwmL);
}
if( pwmR < 0 )
{
pwmR = -pwmR;
if( pwmR >255 ) pwmR = 255;
analogWrite(IN3,0);
analogWrite(IN4,pwmR);
}
else
{
if( pwmR >255 ) pwmR = 255;
analogWrite(IN4,0);
analogWrite(IN3,pwmR);
}
digitalWrite(ENA,HIGH);
digitalWrite(ENB,HIGH);
}
//-------------------------------------------------------------------------
void stopCar()
{
digitalWrite(ENA,LOW);
digitalWrite(ENB,LOW);
}
//-------------------------------------------------------------------------
void blueTooth()
{
if (Serial.available())
{
data = Serial.read();
if( data != lastData ) initialization();
switch(data)
{
case 1: k = 0.5 ;
k2 = 0.031 ;
k3 = 0.0 ;
//setPoint = -2.68 ;
turnSpeed = 0.0 ;
kp = 36.0 ;
ki = 0.0 ;
kd = 2.4415 ;
kps = 0.0 ;
ksp = 1.921 ;
break;
case 2: k = 0.5 ;
k2 = 0.031 ;
k3 = 0.0 ;
//setPoint = -2.68 ;
turnSpeed = 0.0 ;
kp = 36.0 ;
ki = 0.0 ;
kd = 2.4415 ;
kps = 0.0 ;
ksp = 1.921 ;
break;
case 3: k = 0.0 ;
k2 = 0.0 ;
//k3 = 0.0 ;
setPoint = 0.0 ;
turnSpeed = 30 ;
kp = 36 ;
ki = 0.1 ;
kd = 1.88 ;
kps = 0.0 ;
ksp = 1.3 ;
break;
case 4:k = 0.0 ;
k2 = 0.0 ;
//k3 = 0.0 ;
setPoint = 0.0 ;
turnSpeed = -30 ;
kp = 36 ;
ki = 0.1 ;
kd = 1.88 ;
kps = 0.0 ;
ksp = 1.3 ;
break;
default:k = 0.0 ;
k2 = 0.0 ;
//k3 = 0.0 ;
setPoint = 0.0 ;
turnSpeed = 0.0 ;
kp = 36.0 ;
ki = 0.0 ;
kd = 1.88 ;
kps = 0.145 ;
ksp = 1.36 ;
}
lastData = data;
}
if( data == 1 )
{
if( setPoint != -2.68 )
setPoint -= 0.005;
if( setPoint < -2.68 )
{
setPoint = -2.68;
//initialSp = setPoint;
//data = 10;
}
}
if( data == 2 )
{
if( setPoint != 2.68 )
setPoint += 0.005;
if( setPoint > 2.68 )
{
setPoint = 2.68;
//initialSp = setPoint;
//data = 10;
}
}
}
//-----------------------------------------------------------------
void encoder()
{
if( digitalRead(12) == LOW )
{
count ++;
}
else
count --;
if( count > 50000 || count < -50000 )
{
count = 0;
}
}
void encoder2()
{
if( digitalRead(11) == LOW )
{
count2 ++;
}
else
count2 --;
if( count2 > 50000 || count2 < -50000 )
{
count2 = 0;
}
}
void initialization()
{
itermIn = 0;
count = 0;
count2 = 0;
lastPosition = 0;
}
/*
void setPointAdj()
{
if( data == 10 )
{
if( acceleration > 100 )
{
setPoint -= k3 * acceleration;
}
else if( acceleration < -100 )
{
setPoint -= k3 * acceleration;
}
else setPoint = initialSp;
}
}
*/
链接地址：http://pan.baidu.com/s/1hqvJ2cs 有教程不，，， 求教程。。。。 没图你说个毛线啊 慢、节奏 发表于 2014-9-24 21:57 static/image/common/back.gif
没图你说个毛线啊
视频已上传 努力微笑 发表于 2014-9-16 23:31 static/image/common/back.gif
有教程不，，，
帖子已更新 胡萝卜喜欢怪嘎 发表于 2014-9-20 10:21 static/image/common/back.gif
求教程。。。。
帖子已更新 亲，你手机遥控它到处跑是通过改变程序中的哪个值实现的，我的那个可以平衡，但是如果改变位置值是它到处跑，车立马就倒，我想是因为变得太突然了 给个接线图呗，我小白，谢谢 感谢楼主无私分享，学习了！！ 我最近也在做，PCB板刚到，等弄好了，也发帖。 你使用的电机是带码盘的吗？求详细硬件链接图。邮箱[email protected] MPU6050部分如何接线呢？以及光电编码电机AB两相如何介入nano中进行鉴别正反转的？ 楼主我是新人求带也是大二:) 楼主加个QQ不懂的程序问问 好吗javascript:;
页:
[1]
2