设为首页收藏本站

极客工坊

 找回密码
 注册

只需一步,快速开始

查看: 70422|回复: 174

arduino摇控平衡车m2560+mpu6050+24l01 (已上传代码包)

    [复制链接]
发表于 2012-7-17 01:27:43 | 显示全部楼层 |阅读模式
本帖最后由 pww999 于 2015-10-24 11:56 编辑

--------------------------------------------代码包在最下面----------------------------------------------

增加mano328 + 24l01  2.4G摇控功能

原卡耳漫滤波 PD融合控制,改成了互补滤波融合 PID控制,效果也很好

没有限速,会越跑越快,,手动摇控修正{:soso_e113:}

慢慢折腾迟点加代码 快速启动后挂档减速 或 码盘修正?




换了85mm轮子,可以长时间平行,就是推动时角度大了还是会向一边倒....


原65mm轮




搜索了相关资料, 代码在6楼

arduino 2560(主控)+mpu6050(陀螺仪+加速度)+l298n +12V减速电机

整车加电池后788克(太重了~)25的减速电机还是扭力不够~ 可能换80mm的轮子才行了
(建议大家还是买大扭力的减速电机,普通减速电机扭力还是欠佳!!)

电机装上轮子后发现有差不多3度间隙回差~~~~~~,松动~`

电机没装测速传感,不是很稳,

l298n电机驱动没有装光耦,直接2560

为使电压更稳定  12V 电源板加了2只1000uf大电解及2只104 103小瓷片滤波 L298供电,后经7809供电给m2560





附24L01教程:
http://www.geek-workshop.com/for ... thread&tid=1228

相关资料:


http://www.amobbs.com/thread-4100175-1-1.html


65mm轮

65mm轮

换85mm轮

换85mm轮

85mm轮站立视频,稳定多了

85mm轮站立视频,稳定多了

ping_client_char.zip

8.25 KB, 下载次数: 1735

摇控器代码

MPU6050_KK0827_pid.zip

58.41 KB, 下载次数: 3798

小车代码

I2Cdev.zip

13.14 KB, 下载次数: 1513

解压后放到arduino-1.0.1\libraries里面

i2cdevlib-master.zip

200.29 KB, 下载次数: 1715

这个包晗很多常用的传感器的库文件包,解压后把i2cdev放到arduino-1.0.1\libraries里面

回复

使用道具 举报

发表于 2012-7-17 08:49:39 | 显示全部楼层
哇,两个轮子居然能维持平衡!!
回复 支持 反对

使用道具 举报

发表于 2012-7-17 08:55:59 | 显示全部楼层
支持啊,自平衡小车,很库!
回复 支持 反对

使用道具 举报

发表于 2012-7-17 14:55:22 | 显示全部楼层
酷~~~~~~~~~
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-7-20 22:31:17 | 显示全部楼层
本帖最后由 pww999 于 2012-9-8 08:48 编辑

注:
1. 24l01(可以改成按扭高电平时发送,)有时会不能正常发送,须按F位键重启nano,(自行优化代码或用其它摇控模块)
2. 适当调节 前后左右 各值,调整速度,(或增加码盘限速 或 利用PWM输出一定值后--)
3. PID  取样时间值自行调整最佳状态,使坚直更稳定, 适当调整PID各值最用电位器调试
   (也可以用官方自带PID库试试)




回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-7-21 11:37:11 | 显示全部楼层
本帖最后由 pww999 于 2012-11-23 09:56 编辑

//卡耳漫滤波 PD融合控制代码:

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

MPU6050 accelgyro;
//#include <LiquidCrystal.h>;
//LiquidCrystal lcd( 12, 11, 10, 9, 8,7);

#define Gry_offset -20     // 陀螺仪偏移量
//#define Gyr_Gain 0.04      // 满量程2000dps时灵敏度(dps/digital)
#define Gyr_Gain 65.5
#define pi 3.14159

int16_t ax, ay, az;
int16_t gx, gy, gz;


/********** 互补滤波器参数 *********/
//unsigned long preTime = 0; // 采样时间
//float f_angle = 0.0;       // 滤波处理后的角度值


/*********** PID控制器参数 *********/
//unsigned long lastTime;
float Output;   //;, Setpoint,Input;
//double errSum, lastErr;
float kp, ki, kd,kpp;
//int SampleTime = 0.1; //1 sec
//float Outputa = 0.0;  
float angleA,omega;
//double Kp, Ki, Kd;
float P[2][2] = {{ 1, 0 },{ 0, 1 }};
float Pdot[4] ={ 0,0,0,0};
static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5,dtt=0.007,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
float angle, angle_dot;   // aaxdot,aax;
float position_dot,position_dot_filter,positiono;
//double Speed_Need=0;

//float K_angle=2;
//float K_angle_dot=0.5;       //换算系数:256/10 =25.6;
//float K_position=0.1;                //换算系数:(256/10) * (2*pi/(64*12))=0.20944;//256/10:电压换算至PWM,256对应10V;       
//float K_position_dot=1;                //换算系数:(256/10) * (25*2*pi/(64*12))=20.944;

//--------------------------------------

int oommm;
int ooommm;
//int oooommmm;
//double OLH= 10,ORH = 10;

int TN1=22;
int TN2=23;
int TN3=24;
int TN4=25;
int ENA=2;
int ENB=3;

//-------------------------------------
void setup() {
Wire.begin();
//lcd.begin(16, 2);
//lcd.print("hello, world!");
//delay(1000);

accelgyro.initialize();
    delay(500);
pinMode(22,OUTPUT);         
    pinMode(23,OUTPUT);
      pinMode(24,OUTPUT);
        pinMode(25,OUTPUT);
              pinMode(2,OUTPUT);
        pinMode(3,OUTPUT);
        
delay(100);
//Serial.begin(9600);
}

void loop() {
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  //-----------------------------------------------------------------------------------------------------------------

  angleA= atan2(ay , az) * 180 / pi-0.2;  
// 根据加速度分量得到的角度(degree)
//180度至0至-180(360度)取0度为坚直时中立点 因为坚直时为有偏差,所以减去0.2度....
  //omega=  Gyr_Gain * (gx +  Gry_offset); // 当前角速度(degree/s)
   omega=(gx +  Gry_offset)/Gyr_Gain; // 当前角速度(degree/s)
  if (abs(angleA)<30) {    //这个是误差达到一定程度后的系统关闭开关.

Kalman_Filter(angleA,omega);
PIDD();
PWMB();
//LCDD();
   }
    else {
   
analogWrite(ENA, 0); //PWM调速a==0-255
analogWrite(ENB, 0);
  }
     delay(10);
}
//=---------------------------------------------------------------


void Kalman_Filter(double angle_m,double gyro_m)
{
angle+=(gyro_m-q_bias) * dtt;
Pdot[0]=Q_angle - P[0][1] - P[1][0];
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] * dtt;
P[0][1] += Pdot[1] * dtt;
P[1][0] += Pdot[2] * dtt;
P[1][1] += Pdot[3] * dtt;
angle_err = angle_m - angle;
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
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[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
angle+= K_0 * angle_err;
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;//也许应该用last_angle-angle
}

//-----------------------
void  PIDD(){
        kp=analogRead(8)*0.01;
        ki=analogRead(9)*0.007;
        kd=analogRead(10)*0.007;
        kpp=analogRead(11)*0.007;
//aaxdot=0.5*aaxdot+0.5*angle_dot;
//aax=0.5*aax+0.5*angle;
        position_dot=Output*0.04;                //利用PWM值代替轮速传感器的信号
        position_dot_filter*=0.9;                //车轮速度滤波
        position_dot_filter+=position_dot*0.1;       
        positiono+=position_dot_filter;       
        //positiono+=Speed_Need;         
  positiono=constrain(positiono,-500,500);
  Output= 2*angle *kp + 0.5*angle_dot*ki +0.02*positiono *kd + 1*position_dot_filter *kpp;
    //Output= K_angle*angle *kp + K_angle_dot*angle_dot *ki +K_position*positiono *kd + K_position_dot*position_dot_filter *kpp;
}
//-------------电机正反转 PWM输出-----------
void PWMB(){
  if(Output<0)
{

    digitalWrite(TN1, HIGH);
    digitalWrite(TN2, LOW);
     digitalWrite(TN3, HIGH);
    digitalWrite(TN4, LOW);

}
  if(Output>0)
{

    digitalWrite(TN1, LOW);
    digitalWrite(TN2, HIGH);
     digitalWrite(TN3, LOW);
    digitalWrite(TN4, HIGH);  
}
    oommm=min(220,abs(Output));
    analogWrite(ENA, oommm+35); //PWM调速a==0-255
    analogWrite(ENB, oommm+30);
}
回复 支持 反对

使用道具 举报

发表于 2012-7-26 20:42:17 | 显示全部楼层
简介资料下载后没法打开,求救!!
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-7-29 08:51:47 | 显示全部楼层
SZYWJ 发表于 2012-7-26 20:42
简介资料下载后没法打开,求救!!

选PDF格式打开
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-8-3 00:42:11 | 显示全部楼层
本帖最后由 pww999 于 2012-8-21 11:05 编辑

换了85mm的轮子,可以长时间平行,就是推动时角度大了还是会向一边倒....
回复 支持 反对

使用道具 举报

发表于 2012-8-17 22:17:15 | 显示全部楼层
{:soso_e154:}楼主,请问一下,以下:

#define Gry_offset -20     // 陀螺仪偏移量
//#define Gyr_Gain 0.04      // 满量程2000dps时灵敏度(dps/digital)
#define Gyr_Gain 65.5

这些参数应该按照怎样去调试呀~
回复 支持 反对

使用道具 举报

高级模式  
您需要登录后才可以回帖 登录 | 注册  

本版积分规则

Archiver|联系我们|极客工坊 ( 浙ICP备09023225号 )  

GMT+8, 2017-9-23 04:38 , Processed in 0.049778 second(s), 11 queries , File On.

Powered by Discuz! X3.3 Licensed

© 2001-2017 Comsenz Inc.

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