|
本帖最后由 jkhjt1949 于 2016-2-19 12:09 编辑
我是看到论坛里的视频后才萌生DIY平衡小车的。本人大二党,花了2个月才做到这个程度的,完全出于兴趣,拿出来就是想跟大家交流交流。不好意思,文件和图片上传好多次都不成功。在末尾我留了网盘链接了,里面有材料清单和小车资料,有需要的可以下载。欢迎交流!!!
为了偷懒,视频是用4s拍的,效果一般,屏幕较窄,请见谅。
此小车因为安装了蓝牙模块,可用android手机操控,完成前进后退,左右方向旋转动作
详细图片
[pre lang="arduino"]
#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 ;
float pwm, pwmL, pwmR, dt, _speed,lastSpeed,acceleration ;
float drift,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;
}
}
*/
[/code]
链接地址:http://pan.baidu.com/s/1hqvJ2cs |
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
|