Arduino 平衡车 MPU6050 L298N
本帖最后由 天上人间 于 2018-2-12 17:35 编辑L298N电机驱动模块
[*]图示：
http://images.ncnynl.com/ros/2016/20160406161534992.png
[*]驱动电压5v~35v, 逻辑电压5v. 内置的78M05通过驱动电源部分取电工作, 当使用大于12V驱动电压的时候，为了避免稳压芯片损坏, 请使用外置的5V逻辑供电。
[*]ENA：OUT1和OUT2的使能端，默认情况下，跳线帽接上时，高电平有效，低电平禁止；如果需要PWM调速，就需要拔掉跳线帽.
驱动直流电机
[*]本模块是2路的H桥驱动，所以可以同时驱动两个电机
[*]使能ENA ENB之后
[*]可以分别从IN1 IN2输入PWM信号驱动电机1的转速和方向
[*]可以分别从IN3 IN4输入PWM信号驱动电机2的转速和方向
[*]图示：
http://images.ncnynl.com/ros/2016/20160406161619258.png
驱动步进电机
[*]图示：
http://images.ncnynl.com/ros/2016/20160406161917556.png
L298N驱动板介绍：
[*]图示1：
http://images.ncnynl.com/ros/2016/20160406161934337.jpeg图示2：http://images.ncnynl.com/ros/2016/20160406161948791.jpeg
[*]6，7帽拔掉后，插在下面的针上，int1,int2控制左输出1。
[*]如果L298N和arduino使用不同的电源供电的话，那么需要将arduino的GND和模块上的GND连接在一起，只有这样单片机上过来的逻辑信号才有个参考0点。此点非常重要，请大家注意.
L298N驱动板接线方法
[*]方式1：ENA插帽拔掉，通过IN1和IN2控制电机转动方向(正转或反转), 通过ENA控制电机的转速。
测试电机开始转的PWM值 1、PWM值小，电机不转。
2、 PWM就是脉宽调制器，通过调制器给电机提供一个具有一定频率的脉冲宽度可调的脉冲电。脉冲宽度越大即占空比越大，提供给电机的平均电压越大，电机转速就高。反之脉冲宽度越小，则占空比越越小。提供给电机的平均电压越小，电机转速就低。 PWM不管是高电平还是低电平时电机都是转动的，电机的转速取决于平均电压！
注：当电机负载较轻时，PWM 不到100% 就可以让电机达到最快转速，之后 PWM 的改变就不再影响电机速度了（这个概念还是在玩 LEGO 时看到的，LEGO 的 RCX 只能通过 PWM 改变电机速度，所以有时需要额外增加阻力以使得转速可以随 PWM 的变化而变化）。
int output;
void setup()
{
Serial.begin(9600);
for (int i=5; i<=10; i++)//将10，5，6，7，8，9定义为输出口
{
pinMode(i, OUTPUT);
}
}
void loop()
{
for(output=1;output<255;output++){
if (output > 0)
{
digitalWrite(7, LOW);
digitalWrite(8, HIGH);
digitalWrite(9,LOW);
digitalWrite(10, HIGH);
}
if (output < 0)
{
digitalWrite(7, HIGH);
digitalWrite(8, LOW);
digitalWrite(9,HIGH);
digitalWrite(10, LOW);
}
//output = min(200,abs(40*angleG));//此函数输出较小值，将PWM最大值限定在200，下面起步补偿55
analogWrite(5, output);//左右轮都补偿起步PWM值，左右轮电机可能不一致，有可能5针ENA右轮多补偿
analogWrite(6, output);//output的值在100左右时，香蕉电机才起动，汗！。
delay(1000);
Serial.print("output = ");
Serial.println(output);
}
}MPU6050
https://pic2.zhimg.com/681ec672d3f1b82f2e1f28d94e17537a_r.jpg
[*] MPU6050是一种非常流行的空间运动传感器芯片，可以获取器件当前的三个加速度分量和三个旋转角速度。
[*] MPU6050的数据接口用的是I2C总线协议，因此我们需要Wire程序库的帮助来实现Arduino与MPU6050之间的通信。请先确认你的Arduino编程环境中已安装Wire库。
[*]Wire库的官方文档（http://www.arduino.cc/en/Reference/Wire）中指出：在UNO板子上，SDA接口对应的是A4引脚，SCL对应的是A5引脚。MPU6050需要5V的电源，可由UNO板直接供电。按照下图连线。
[*]MPU6050 引脚说明VCC 3.3-5V（内部有稳压芯片）GND 地线
SCL MPU6050作为从机时IIC时钟线SDA MPU6050作为从机时IIC数据线XCL MPU6050作为主机时IIC时钟线XDA MPU6050作为主机时IIC数据线
AD0 地址管脚，该管脚决定了IIC地址的最低一位INT 中断引脚https://pic3.zhimg.com/80/ad102a2c0e2f903d86a0c8dc79d81dc4_hd.jpg
（紫色线是中断线）
程序实现
[*]首先要更新I2C库
[*]在GITHUB找到的I2C库
[*]（程序来源: https://github.com/jrowberg/i2cdevlib）打开,把Arduino文件夹里的I2Cdev，MPU6050文件夹复制到Arduino IDE的库文件夹里
[*](默认的路径是这个 C:\Program Files (x86)\Arduino\libraries)(本人把这些文件放在了源文件.ino所在文件夹)
[*]在GITHUB找到的卡尔曼滤波程序（程序来源: https://github.com/wjjun/MPU6050_Kalman）
[*]把程序上传到板子上，打开串口监视器，就可以看到一堆堆的数据了
卡尔曼滤波器
简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。
卡尔曼滤波器的介绍 ：
为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。
在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。
假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分布（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。
好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。
假如我们要估算k时刻的实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。
由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的协方差(covariance)来判断。因为Kg=5^2/(5^2+4^2)，所以Kg=0.61，我们可以估算出k时刻的实际温度值是：23+0.61*(25-23)=24.22度。可以看出，因为温度计的协方差(covariance)比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。
现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.22度）的偏差。算法如下：((1-Kg)*5^2)^0.5=3.12。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的3.12就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。
就是这样，卡尔曼滤波器就不断的把(协方差(covariance)递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的协方差(covariance)。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值，是不是很神奇！
http://image.geek-workshop.com/forum/201712/29/200309cmsucdmm7wh8emni.png
//卡尔曼滤波参数与函数
float dt=0.005;//注意：dt的取值为kalman滤波器采样时间
float angle, angle_dot;//角度和角速度
float P = {{ 1, 0 },
{ 0, 1 }};
float Pdot ={ 0,0,0,0};
float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度
float R_angle=0.5 ,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//卡尔曼滤波
void Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy
{
angle+=(gyro_m-q_bias) * dt;
angle_err = angle_m - angle;
Pdot=Q_angle - P - P;
Pdot=- P;
Pdot=- P;
Pdot=Q_gyro;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
PCt_0 = C_0 * P;
PCt_1 = C_0 * P;
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;
P -= K_0 * t_0;
P -= K_0 * t_1;
P -= K_1 * t_0;
P -= K_1 * t_1;
angle += K_0 * angle_err; //最优角度
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;//最优角速度
}
陀螺仪 简单读取
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
bool blinkState = false;
void setup() {
Wire.begin();
Serial.begin(38400);
accelgyro.initialize();
}
void loop() {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Serial.print("a/g:\t");
Serial.print(ax); Serial.print("\t");
Serial.print(ay); Serial.print("\t");
Serial.print(az); Serial.print("\t");
Serial.print(gx); Serial.print("\t");
Serial.print(gy); Serial.print("\t");
Serial.println(gz);
blinkState = !blinkState;
}
Arduino uno + mpu6050 陀螺仪 运用卡尔曼滤波姿态解算
本例程输出XYZ的角度，正负90度。运用卡尔曼滤波算法解算姿态。
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
unsigned long now, lastTime = 0;
float dt; //微分时间
int16_t ax, ay, az, gx, gy, gz; //加速度计陀螺仪原始数据
float aax=0, aay=0,aaz=0, agx=0, agy=0, agz=0; //角度变量
long axo = 0, ayo = 0, azo = 0; //加速度计偏移量
long gxo = 0, gyo = 0, gzo = 0; //陀螺仪偏移量
float pi = 3.1415926;
float AcceRatio = 16384.0; //加速度计比例系数
float GyroRatio = 131.0; //陀螺仪比例系数
uint8_t n_sample = 8; //加速度计滤波算法采样个数
float aaxs = {0}, aays = {0}, aazs = {0}; //x,y轴采样队列
long aax_sum, aay_sum,aaz_sum; //x,y轴采样和
float a_x={0}, a_y={0},a_z={0} ,g_x={0} ,g_y={0},g_z={0}; //加速度计协方差计算队列
float Px=1, Rx, Kx, Sx, Vx, Qx; //x轴卡尔曼变量
float Py=1, Ry, Ky, Sy, Vy, Qy; //y轴卡尔曼变量
float Pz=1, Rz, Kz, Sz, Vz, Qz; //z轴卡尔曼变量
void setup()
{
Wire.begin();
Serial.begin(115200);
accelgyro.initialize(); //初始化
unsigned short times = 200; //采样次数
for(int i=0;i<times;i++)
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值
axo += ax; ayo += ay; azo += az; //采样和
gxo += gx; gyo += gy; gzo += gz;
}
axo /= times; ayo /= times; azo /= times; //计算加速度计偏移
gxo /= times; gyo /= times; gzo /= times; //计算陀螺仪偏移
}
void loop()
{
unsigned long now = millis(); //当前时间(ms)
dt = (now - lastTime) / 1000.0; //微分时间(s)
lastTime = now; //上一次采样时间(ms)
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值
float accx = ax / AcceRatio; //x轴加速度
float accy = ay / AcceRatio; //y轴加速度
float accz = az / AcceRatio; //z轴加速度
aax = atan(accy / accz) * (-180) / pi; //y轴对于z轴的夹角
aay = atan(accx / accz) * 180 / pi; //x轴对于z轴的夹角
aaz = atan(accz / accy) * 180 / pi; //z轴对于y轴的夹角
aax_sum = 0; // 对于加速度计原始数据的滑动加权滤波算法
aay_sum = 0;
aaz_sum = 0;
for(int i=1;i<n_sample;i++)
{
aaxs = aaxs;
aax_sum += aaxs * i;
aays = aays;
aay_sum += aays * i;
aazs = aazs;
aaz_sum += aazs * i;
}
aaxs = aax;
aax_sum += aax * n_sample;
aax = (aax_sum / (11*n_sample/2.0)) * 9 / 7.0; //角度调幅至0-90°
aays = aay; //此处应用实验法取得合适的系数
aay_sum += aay * n_sample; //本例系数为9/7
aay = (aay_sum / (11*n_sample/2.0)) * 9 / 7.0;
aazs = aaz;
aaz_sum += aaz * n_sample;
aaz = (aaz_sum / (11*n_sample/2.0)) * 9 / 7.0;
float gyrox = - (gx-gxo) / GyroRatio * dt; //x轴角速度
float gyroy = - (gy-gyo) / GyroRatio * dt; //y轴角速度
float gyroz = - (gz-gzo) / GyroRatio * dt; //z轴角速度
agx += gyrox; //x轴角速度积分
agy += gyroy; //x轴角速度积分
agz += gyroz;
/* kalman start */
Sx = 0; Rx = 0;
Sy = 0; Ry = 0;
Sz = 0; Rz = 0;
for(int i=1;i<10;i++)
{ //测量值平均值运算
a_x = a_x; //即加速度平均值
Sx += a_x;
a_y = a_y;
Sy += a_y;
a_z = a_z;
Sz += a_z;
}
a_x = aax;
Sx += aax;
Sx /= 10; //x轴加速度平均值
a_y = aay;
Sy += aay;
Sy /= 10; //y轴加速度平均值
a_z = aaz;
Sz += aaz;
Sz /= 10;
for(int i=0;i<10;i++)
{
Rx += sq(a_x - Sx);
Ry += sq(a_y - Sy);
Rz += sq(a_z - Sz);
}
Rx = Rx / 9; //得到方差
Ry = Ry / 9;
Rz = Rz / 9;
Px = Px + 0.0025; // 0.0025在下面有说明...
Kx = Px / (Px + Rx); //计算卡尔曼增益
agx = agx + Kx * (aax - agx); //陀螺仪角度与加速度计速度叠加
Px = (1 - Kx) * Px; //更新p值
Py = Py + 0.0025;
Ky = Py / (Py + Ry);
agy = agy + Ky * (aay - agy);
Py = (1 - Ky) * Py;
Pz = Pz + 0.0025;
Kz = Pz / (Pz + Rz);
agz = agz + Kz * (aaz - agz);
Pz = (1 - Kz) * Pz;
/* kalman end */
Serial.print(agx);Serial.print(","); //要看MPU6050如何放置<span style="color: rgb(0, 0, 0); font-family: Arial, Helvetica, sans-serif; font-size: 12px; background-color: rgb(255, 255, 255);">翻转角 俯仰角和偏航角</span>
Serial.print(agy);Serial.print(",");
Serial.print(agz);Serial.println();
}
PID控制
1、比例（P）控制
比例控制是一种最简单的控制方式。其控制器的输出与输入误差信号成比例关系。当仅有比例控制时系统输出存在稳态误差（Steady-state error）。
2、 积分（I）控制
在积分控制中，控制器的输出与输入误差信号的积分成正比关系。对一个自动控制系统，如果在进入稳态后存在稳态误差，则称这个控制系统是有稳态误差的或简称有差系统（System with Steady-state Error）。为了消除稳态误差，在控制器中必须引入“积分项”。积分项对误差取决于时间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零。因此，比例+积分(PI)控制器，可以使系统在进入稳态后无稳态误差。
3、微分（D）控制
在微分控制中，控制器的输出与输入误差信号的微分（即误差的变化率）成正比关系。自动控制系统在克服误差的调节过程中可能会出现振荡甚至失稳。其原因是由于存在有较大惯性组件（环节）或有滞后(delay)组件，具有抑制误差的作用，其变化总是落后于误差的变化。解决的办法是使抑制误差的作用的变化“超前”，即在误差接近零时，抑制误差的作用就应该是零。这就是说，在控制器中仅引入 “比例”项往往是不够的，比例项的作用仅是放大误差的幅值，而目前需要增加的是“微分项”，它能预测误差变化的趋势，这样，具有比例+微分的控制器，就能够提前使抑制误差的控制作用等于零，甚至为负值，从而避免了被控量的严重超调。所以对有较大惯性或滞后的被控对象，比例+微分(PD)控制器能改善系统在调节过程中的动态特性。
最终用的这个代码，站的不好，只站一会儿:'(
//copyright by kaiser 20140521 V1.0
/////////////////////////////////////////////////////////////////////////////////////////////////////
#include "Wire.h" //serial
#include "I2Cdev.h" //IIC
#include "MPU6050.h" //acc&gyro Sensor
//Define Variables we'll be connecting to
char val='s';int Speed_need=0;int Turn_need=0;
float speeds,speeds_filter, positions;
float diff_speeds,diff_speeds_all;
////////////////////PID parameter///////////////////////////////////////
double Output=0;
float Kp=10,Kd=0.06,Ksp = 2.8,Ksi = 0.11; //
////////////////////////////////////////////////
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
/********************角度参数**************/
float Gyro_y; //Y轴陀螺仪数据暂存
float Angle_ax; //由加速度计算的倾斜角度
float Angle; //小车最终倾斜角度
//int Setpoint=0;
////////////////////////////////////////Pin assignment///////////////////////////////////////
int PinA_right= 2; //interrupt 0
int PinA_left= 3; //interrupt 1
int E_left =6;//ENA
int M_right =7;
int M_right2=8;
int E_right =5; //ENB
int M_left =9;
int M_left2 =10;
//////////////////////////////////////////////////////////////////////////////
int PWM_right=0; int PWM_left=0;
int PWM_left_least=87; int PWM_right_least=88;//left:77,right:78
///////////////////////////////interrupt for Speed/////////////////////////////////
int count_right =0;
int count_left=0;
int old_time=0;
int flag;
void Code_right(){if(Output>=0){count_right += 1;}else{count_right -= 1;} }//if only +,can't stand up 编码器码盘计数加一
void Code_left(){if(Output>=0){count_left += 1;} else{count_left -= 1;}}// 编码器码盘计数加一
/////////////////////////Right&Left&Stop///////////////////////////////////////////////
void SetMotorVoltage(int nLeftVol, int nRightVol) {
if(nLeftVol >=0)
{digitalWrite(M_left,LOW);
digitalWrite(M_left2,HIGH);
} else {
digitalWrite(M_left,HIGH);
digitalWrite(M_left2,LOW);
nLeftVol=-nLeftVol;
}
if(nRightVol >= 0) {
digitalWrite(M_right,LOW);
digitalWrite(M_right2,HIGH);
} else {
digitalWrite(M_right,HIGH);
digitalWrite(M_right2,LOW);
nRightVol=-nRightVol;
}
if(nLeftVol>255) { nLeftVol = 255 ; } //防止PWM值超过255
if(nRightVol>255) { nRightVol = 255 ; }//防止PWM值超过255
analogWrite(E_left,nLeftVol);
analogWrite(E_right,nRightVol);
}
/////////////////////////////////////////////////////////////////////////////////////////////
void Angle_Calcu(void) {
Angle_ax = (ax+1942)/238.2 ; //去除零点偏移1942,//16384*3.14/1.2*180//弧度转换为度,
Gyro_y = -(gy-119.58)/16.4; //去除零点偏移119,2000deg/s 16.4 LSB/(deg/s)250---131 ，负号为方向处理
//Serial.print("Angle_ax,Angle_gy");Serial.print(Angle_ax);Serial.println(Angle_gy);
Angle=0.97*(Angle+Gyro_y*0.0005)+0.03*Angle_ax;
//Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角
}
void setup()
{
Wire.begin();
Serial.begin(9600);
///////////////////////////////////////////////////////////////////
accelgyro.reset();//reset
delay(1);
accelgyro.setClockSource(MPU6050_CLOCK_PLL_YGYRO);//PLL with Y Gyro reference*/
accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);//0x1B Value 0x18 2000°/s
accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_4);//0x1C Value 0x18 16g
accelgyro.setDLPFMode(MPU6050_DLPF_BW_5);//0x06 means acc 5Hz delay19.0ms Gyro 5Hz delay 18.6ms
accelgyro.setTempSensorEnabled(true);//disable temsensor
accelgyro.setSleepEnabled(false);
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
////////////////////////pin mode////////////////////////////////////////
pinMode(E_right, OUTPUT); pinMode(M_right, OUTPUT);pinMode(M_right2, OUTPUT);//right
pinMode(E_left, OUTPUT);pinMode(M_left, OUTPUT);pinMode(M_left2, OUTPUT);//left
pinMode(PinA_right,INPUT);pinMode(PinA_left,INPUT);// in 0 in 1
Serial.println("Pin mode ...");
/////////////////////////////interrupt/////////////////////////////////////////////
attachInterrupt(0, Code_right, FALLING);attachInterrupt(1, Code_left, FALLING);
}
void loop()
{
if (Serial.available() > 0){val = Serial.read();Serial.println(val);}
switch(val){
case 'a':Speed_need=30;Turn_need=0;positions=80;break;//Go
case 'b':Speed_need=10;Turn_need=-10;positions=10;break;//right
case 'c':Speed_need=10;Turn_need=10;positions=10;break;//left
case 'd':Speed_need=0;Turn_need=0;positions=0;break;
default:Speed_need=0;Turn_need=0;positions=0;break;}//stop
//Speed_need=30;Turn_need=0;positions=80;
//SetMotorVoltage(255,255);
//Kp=15,Kd=0.09,Ksp = 2.8,Ksi = 0.11;
//Serial.print("count_left");Serial.println(count_left);
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Angle_Calcu();
//Serial.print("Angle");Serial.println(Angle);
PWM_Calcu();
//if(millis()-old_time>=500){ Serial.print("count_right");Serial.print(count_right);Serial.print("count_left");Serial.println(count_left);old_time=millis();count_right=0;count_left=0;}
}
void PWM_Calcu(void)
{
if (abs(Angle)>40)
{SetMotorVoltage(0,0);}//角度大于45度 停止PWM输出
else
{ //Speed_need=30;Turn_need=0;positions=80;
//////////////////////
speeds=(count_left + count_right)*0.5;
diff_speeds = count_left - count_right;
diff_speeds_all += diff_speeds;
speeds_filter *=0.85;//一阶互补滤波
speeds_filter +=speeds*0.15;
positions += speeds_filter;
positions += Speed_need;
positions = constrain(positions, -2300, 2300);//抗积分饱和
////////////////////
Output=Kp*Angle+Kd*Gyro_y+ Ksp*speeds_filter + Ksi*positions ;
//Serial.print("Output");Serial.println(Output);
if(Turn_need==0){PWM_right=Output-diff_speeds_all;
PWM_left=Output+diff_speeds_all;}
PWM_right=Output+Turn_need;
PWM_left=Output-Turn_need;
if(PWM_right>=0){PWM_right+=PWM_right_least;}else{PWM_right-=PWM_right_least;}
if(PWM_left>=0){PWM_left+=PWM_left_least;}else{PWM_left-=PWM_left_least;}
SetMotorVoltage(PWM_left,PWM_right);}
count_left = 0;
count_right = 0;
}
修改了几个地方
//copyright by kaiser 20140521 V1.0
/////////////////////////////////////////////////////////////////////////////////////////////////////
#include "Wire.h" //serial
#include "I2Cdev.h" //IIC
#include "MPU6050.h" //acc&gyro Sensor
//Define Variables we'll be connecting to
char val='s';int Speed_need=0;int Turn_need=0;
float speeds,speeds_filter, positions;
float diff_speeds,diff_speeds_all;
////////////////////PID parameter///////////////////////////////////////
double Output=0;
float Kp=10,Kd=0.06,Ksp = 2.8,Ksi = 0.11; //
////////////////////////////////////////////////
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
/********************角度参数**************/
float Gyro_y; //Y轴陀螺仪数据暂存
float Angle_ax; //由加速度计算的倾斜角度
float Angle; //小车最终倾斜角度
//int Setpoint=0;
////////////////////////////////////////Pin assignment///////////////////////////////////////
int PinA_right= 2; //interrupt 0
int PinA_left= 3; //interrupt 1
int E_left =6;//ENA
int M_right =7;
int M_right2=8;
int E_right =5; //ENB
int M_left =9;
int M_left2 =10;
//////////////////////////////////////////////////////////////////////////////
int PWM_right=0; int PWM_left=0;
int PWM_left_least=87; int PWM_right_least=88;//left:77,right:78
///////////////////////////////interrupt for Speed/////////////////////////////////
int count_right =0;
int count_left=0;
int old_time=0;
int flag;
int moto=85;
int qd=0;
void Code_right(){if(Output>=0){count_right += 1;}else{count_right -= 1;} }//if only +,can't stand up 编码器码盘计数加一
void Code_left(){if(Output>=0){count_left += 1;} else{count_left -= 1;}}// 编码器码盘计数加一
/////////////////////////Right&Left&Stop///////////////////////////////////////////////
void SetMotorVoltage(int nLeftVol, int nRightVol) {
if(nLeftVol >=0)
{digitalWrite(M_left,LOW);
digitalWrite(M_left2,HIGH);
} else {
digitalWrite(M_left,HIGH);
digitalWrite(M_left2,LOW);
nLeftVol=-nLeftVol;
}
if(nRightVol >= 0) {
digitalWrite(M_right,LOW);
digitalWrite(M_right2,HIGH);
} else {
digitalWrite(M_right,HIGH);
digitalWrite(M_right2,LOW);
nRightVol=-nRightVol;
}
nLeftVol = nLeftVol+moto ;nRightVol = nRightVol+moto ;
if(nLeftVol>255) { nLeftVol = 255 ; } //防止PWM值超过255
if(nRightVol>255) { nRightVol = 255 ; }//防止PWM值超过255
if(qd==1) { nLeftVol = 0 ;nRightVol = 0 ; } //已倒，PWM值为0
Serial.println(nRightVol);
analogWrite(E_left,nLeftVol);
analogWrite(E_right,nRightVol);
}
/////////////////////////////////////////////////////////////////////////////////////////////
void Angle_Calcu(void) {
Angle_ax = (ax-1300)/238.2 ; //去除零点偏移1942,//16384*3.14/1.2*180//弧度转换为度,
Gyro_y = -(gy-180)/16.4; //去除零点偏移119,2000deg/s 16.4 LSB/(deg/s)250---131 ，负号为方向处理
//Angle_ax = (ax)/238.2 ;
//Gyro_y = -(gy)/16.4;
//Serial.print("Angle_ax,Angle_gy");Serial.print(Angle_ax);Serial.println(Angle_gy);
Angle=0.97*(Angle+Gyro_y*0.0005)+0.03*Angle_ax;
//Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角
//Serial.println(Angle);
}
float dt=0.005;//注意：dt的取值为kalman滤波器采样时间
float angle, angle_dot;//角度和角速度
float P = {{ 1, 0 },
{ 0, 1 }};
float Pdot ={ 0,0,0,0};
float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度
float R_angle=0.5 ,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//卡尔曼滤波
void Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy
{
angle+=(gyro_m-q_bias) * dt;
angle_err = angle_m - angle;
Pdot=Q_angle - P - P;
Pdot=- P;
Pdot=- P;
Pdot=Q_gyro;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
PCt_0 = C_0 * P;
PCt_1 = C_0 * P;
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;
P -= K_0 * t_0;
P -= K_0 * t_1;
P -= K_1 * t_0;
P -= K_1 * t_1;
angle += K_0 * angle_err; //最优角度
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;//最优角速度
}
void setup()
{
Wire.begin();
Serial.begin(9600);
///////////////////////////////////////////////////////////////////
accelgyro.reset();//reset
delay(500);
accelgyro.setClockSource(MPU6050_CLOCK_PLL_YGYRO);//PLL with Y Gyro reference*/
accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);//0x1B Value 0x18 2000°/s
accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_4);//0x1C Value 0x18 16g
accelgyro.setDLPFMode(MPU6050_DLPF_BW_5);//0x06 means acc 5Hz delay19.0ms Gyro 5Hz delay 18.6ms
accelgyro.setTempSensorEnabled(true);//disable temsensor
accelgyro.setSleepEnabled(false);
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
////////////////////////pin mode////////////////////////////////////////
pinMode(E_right, OUTPUT); pinMode(M_right, OUTPUT);pinMode(M_right2, OUTPUT);//right
pinMode(E_left, OUTPUT);pinMode(M_left, OUTPUT);pinMode(M_left2, OUTPUT);//left
pinMode(PinA_right,INPUT);pinMode(PinA_left,INPUT);// in 0 in 1
Serial.println("Pin mode ...");
/////////////////////////////interrupt/////////////////////////////////////////////
attachInterrupt(0, Code_right, FALLING);attachInterrupt(1, Code_left, FALLING);
}
void loop()
{
if (Serial.available() > 0){val = Serial.read();Serial.println(val);}
switch(val){
case 'a':Speed_need=30;Turn_need=0;positions=80;break;//Go
case 'b':Speed_need=10;Turn_need=-10;positions=10;break;//right
case 'c':Speed_need=10;Turn_need=10;positions=10;break;//left
case 'd':Speed_need=0;Turn_need=0;positions=0;break;
default:Speed_need=0;Turn_need=0;positions=0;break;}//stop
//Speed_need=30;Turn_need=0;positions=80;
//SetMotorVoltage(100,100);
//Serial.print("count_left");Serial.println(count_left);
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//Serial.print("a/g:\t");
// Serial.print(ax); Serial.print("\t");
// Serial.print(ay); Serial.print("\t");
// Serial.print(az); Serial.print("\t");
// Serial.print(gx); Serial.print("\t");
//Serial.print(gy); Serial.print("\t");
// Serial.println(gz);
Angle_Calcu();
//Serial.print("Angle");Serial.println(Angle);
PWM_Calcu();
//if(millis()-old_time>=500){ Serial.print("count_right");Serial.print(count_right);Serial.print("count_left");Serial.println(count_left);old_time=millis();count_right=0;count_left=0;}
}
void PWM_Calcu(void)
{
Serial.println(Angle);
if (abs(Angle)>40)
{qd=1;
SetMotorVoltage(0,0);}//角度大于45度 停止PWM输出
else
{ //Speed_need=30;Turn_need=0;positions=80;
//////////////////////
qd=0;
speeds=(count_left + count_right)*0.5;
diff_speeds = count_left - count_right;
diff_speeds_all += diff_speeds;
speeds_filter *=0.85;//一阶互补滤波
speeds_filter +=speeds*0.15;
positions += speeds_filter;
positions += Speed_need;
positions = constrain(positions, -2300, 2300);//抗积分饱和
////////////////////
Output=Kp*Angle+Kd*Gyro_y+ Ksp*speeds_filter + Ksi*positions ;
//Serial.println(Output);
if(Turn_need==0){PWM_right=Output-diff_speeds_all;
PWM_left=Output+diff_speeds_all;}
PWM_right=Output+Turn_need;
PWM_left=Output-Turn_need;
if(PWM_right>=0){PWM_right+=PWM_right_least;}else{PWM_right-=PWM_right_least;}
if(PWM_left>=0){PWM_left+=PWM_left_least;}else{PWM_left-=PWM_left_least;}
//Serial.print(PWM_left);Serial.print("\t");Serial.println(PWM_right);
SetMotorVoltage(PWM_left,PWM_right);}
count_left = 0;
count_right = 0;
}
不如来个视频展示下现阶段的成果是怎么样的
页:
[1]