极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

楼主: smxyk123

Arduino 自平衡小车,制作经验记录与分享,愿有兴趣人士,一起学习探讨。

  [复制链接]
发表于 2014-9-4 17:22:25 | 显示全部楼层
Output=Kp*Angle+Kd*Gyro_y+ Ksp*speeds_filter + Ksi*positions ;   里面的Ksi  Ksp 是什么参数 没有用到I吗?
这是用PD控制的对吗?
回复 支持 反对

使用道具 举报

发表于 2014-9-4 22:03:26 | 显示全部楼层
Processing 图形工具在线调试PID参数教程   这个网址打不开,楼主能给个教程不??
回复 支持 反对

使用道具 举报

发表于 2014-9-5 17:21:18 | 显示全部楼层
smallfivecn 发表于 2014-8-29 10:15
楼主:你的电机测速为何只接了一路呀?每个电机测速都有A、B相输出,接两路不是更准确吗?我用的是TB6612电 ...

恩恩 我也用AB测速  但不知道怎么写代码
回复 支持 反对

使用道具 举报

发表于 2014-9-20 17:15:31 | 显示全部楼层
我的还无法站立。。。
回复 支持 反对

使用道具 举报

发表于 2014-10-9 21:08:56 | 显示全部楼层
楼主,关于.Processing 图形工具在线调试PID的文章已经找不到了,现在我的小车调试调不明白,希望楼主能给些意见和资料让我参考一下,非常感谢你.
回复 支持 反对

使用道具 举报

发表于 2014-10-9 23:24:26 | 显示全部楼层
感谢分享
回复 支持 反对

使用道具 举报

发表于 2014-10-24 11:05:54 | 显示全部楼层
楼主你好,你的pin2和pin3接的是什么输入,暂时搞不懂呀
回复 支持 反对

使用道具 举报

发表于 2014-10-28 22:41:53 | 显示全部楼层
{:soso_e179:}
回复 支持 反对

使用道具 举报

发表于 2014-11-6 12:45:56 | 显示全部楼层
我的怎么也站不起来!
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

#include <FlexiTimer2.h>
//#include <LiquidCrystal_I2C.h>
//LiquidCrystal_I2C lcd(0x27,16,2); //set the LCD address to 0x27 for a 16 chars and 2 line display
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
  
// -----------
#define PinA 2  //中断0
#define PinB 3  //中断1
const int encoderPinA = 4;
const int encoderPinB = 5;

//unsigned long time = 0;
long countA = 0; //计数值
long countB = 0; //计数值
       
//-------------------------
int TN1=7;
int TN2=8;
int ENA=9;

int TN3=12;
int TN4=13;
int ENB=10;

int LOutput;  
int ROutput;
//初始化
char data;
//-----
float angleA,omega;

#define Gry_offset 1     // 陀螺仪偏移量
#define Gyr_Gain 0.00763358    //对应的1G
#define pi 3.14159

float K_angle=4.0;
float K_angle_dot=0.2;       //换算系数: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=4;

//float K_position=0.8 * 0.209;                //换算系数:(256/10) * (2*pi/(64*12))=0.20944;//256/10:电压换算至PWM,256对应10V;
//float K_angle=34 * 25.6;   //换算系数:256/10 =25.6;
//float K_position_dot=1.09 * 20.9;                //换算系数:(256/10) * (25*2*pi/(64*12))=20.944;
//float K_angle_dot=2 * 25.6;                //换算系数:256/10 =25.6;
int Speed_Need;
int Turn_Need;
float Output;

static float kp, ki, kd, kpp;
static float P[2][2] = {{ 1, 0 },{ 0, 1 }};
static float Pdot[4] ={ 0,0,0,0};
//static const float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5,dt=0.01;
static const float Q_angle=0.001, Q_gyro=0.004, R_angle=0.5,dt=0.01;
static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
static const char C_0 = 1;
float position_dot,position_dot_filter,positiono;
float angle, angle_dot;

void setup()
{
   //---------------
Wire.begin();
    Serial.begin(9600);
     accelgyro.initialize();
     delay(100);
    pinMode(TN1,OUTPUT);         
    pinMode(TN2,OUTPUT);
    pinMode(TN3,OUTPUT);
    pinMode(TN4,OUTPUT);
    pinMode(ENA,OUTPUT);
    pinMode(ENB,OUTPUT);   
    //Serial.println("ki ka kb kc");
     delay(100);
   // -----------
    pinMode(PinA,INPUT); //D2脚为输入
    pinMode(PinB,INPUT); //D3脚为输入

    attachInterrupt(0, blinkA,FALLING);  //注册中断0调用函数blinkA
    attachInterrupt(1, blinkB, FALLING);  //注册中断1调用函数blinkB
    pinMode(encoderPinA, INPUT);
    pinMode(encoderPinB, INPUT);
    digitalWrite(encoderPinA, HIGH);
    digitalWrite(encoderPinB, HIGH);
// time = millis(); //时间初值
    delay(100);

FlexiTimer2::set(10, flash); // call every 500 1ms "ticks"
FlexiTimer2::start();
}

void loop()
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//SDEER();
     if (Serial.available()>0)  
  {
        data = Serial.read();
        delay(2);
        switch(data)
        {
         // case ' ':Speed_Need=0;Turn_Need=0;break;
        case 'w':Speed_Need=18;break;
         case 's':Speed_Need=-38;break;
          case 'a':Turn_Need=-30;break;
           case 'd':Turn_Need=30;break;
           default:Speed_Need=0;Turn_Need=0;break;
        }
  }

      PWMB();
      /*  
      Serial.print(angleA);
Serial.print(',');
Serial.print(omega);
Serial.print(',');
Serial.println(Output);
   
    // Serial.print(position_dot);
  //   Serial.print(',');
   
Serial.print(countA);
Serial.print(',');
Serial.print(countB);
Serial.print(',');

*/
    }
//--------------------
void flash()
{
ADOM();
PIDD();
countA=countB=0;
}

//中断0调用函数

void blinkA()
{
// if ((millis() - time) > 3) //防抖动处理
    if (digitalRead(encoderPinA) == HIGH)
    {
       countA ++;
    }
        else
    {
       countA --;
    }
// time = millis();
}

void blinkB()
{
// if ((millis() - time) > 3) //防抖动处理
    if (digitalRead(encoderPinB) == HIGH)
    {
        countB --;
    }
        else
    {
       countB ++;
    }
// time = millis();
}


/*
void SDEER()
{
     if (Serial.available()>0)  
  {
        data = Serial.read();
        delay(2);
        switch(data)
        {
         // case ' ':Speed_Need=0;Turn_Need=0;break;
        case 'w':Speed_Need=18;break;
         case 's':Speed_Need=-38;break;
          case 'a':Turn_Need=-30;break;
           case 'd':Turn_Need=30;break;
           default:Speed_Need=0;Turn_Need=0;break;
        }
      
    // if(data=='w'){ Speed_Need=5;}
   //  else if(data=='s'){ Speed_Need=-5;}
   //  else if(data=='a'){ Turn_Need=20;}
    // else if(data=='d'){ Turn_Need=-20;}
    // else{Speed_Need=0;Turn_Need=0;}
     
    // delay(2);
  }
}
*/  

void ADOM()
{
   angleA= atan2(ay , az) * 180 / pi;   // 根据加速度分量得到的角度(degree)加0.5偏移量
  //180度至0至-180(360度)取0度为坚直时中立点 因为坚直时有偏差,所以加0.5....
   omega=  Gyr_Gain * (gx +  Gry_offset); // 当前角速度(degree/s)
   Kalman_Filter(angleA,omega);
  }
// -------------
void Kalman_Filter(double angle_m,double gyro_m)
{
        angle+=(gyro_m-q_bias) * dt;       
        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] * dt;
        P[0][1] += Pdot[1] * dt;
        P[1][0] += Pdot[2] * dt;
        P[1][1] += Pdot[3] * dt;       
        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;
}
void  PIDD(){
        kp=analogRead(0)*0.005;
        ki=analogRead(1)*0.005;
        kd=analogRead(2)*0.004;
        kpp=analogRead(3)*0.004;
      position_dot=(countA+countB)*0.5;                //利用PWM值代替轮速传感器的信号
      position_dot_filter*=0.8;                //车轮速度滤波
      position_dot_filter+=position_dot*0.2;       
      positiono+=position_dot_filter;          //增加这里
      //position+=position_dot;
      positiono+=Speed_Need;   
      positiono= constrain(positiono, -800, +800);
      Output= K_angle*angle *kp + K_angle_dot*angle_dot *ki +K_position*positiono*kd +K_position_dot*position_dot_filter *kpp;
      LOutput=Output+Turn_Need;
      ROutput=Output-Turn_Need;
}


void PWMB()
{
   if(LOutput>3)  //左电机-------或者取0
{
    digitalWrite(TN1, HIGH);
    digitalWrite(TN2, LOW);
    analogWrite(ENA,min(255,abs(LOutput)+2)); //PWM调速a==0-255
}
  else if(LOutput<-3)//-------或者取0
{
    digitalWrite(TN1, LOW);
    digitalWrite(TN2, HIGH);
    analogWrite(ENA,min(255,abs(LOutput)+2)); //PWM调速a==0-255
}
else
{
    digitalWrite(TN1, LOW);
    digitalWrite(TN2, LOW);
    analogWrite(ENA,0);
}

if(ROutput>3)//右电机--------或者取0
{
    digitalWrite(TN3,LOW);
    digitalWrite(TN4,HIGH);
    analogWrite(ENB,min(255,abs(ROutput)));
  
}
  else if(ROutput<-3)//-------或者取0
{
    digitalWrite(TN3,HIGH);
    digitalWrite(TN4,LOW);
    analogWrite(ENB,min(255,abs(ROutput)));
         
}
else
{
    digitalWrite(TN3, LOW);
    digitalWrite(TN4, LOW);
    analogWrite(ENB,0);
}
//analogWrite(ENA,min(255,abs(LOutput))); //PWM调速a==0-255
//analogWrite(ENB,min(255,abs(ROutput+4)));
}
回复 支持 反对

使用道具 举报

发表于 2014-11-8 18:52:48 | 显示全部楼层
lz,有没有更加详细的接线图?以及那个12V的输入是用的电池咩?
回复 支持 反对

使用道具 举报

发表于 2014-11-9 12:02:11 | 显示全部楼层
楼主牛人啊,好资源果断收藏!
回复 支持 反对

使用道具 举报

发表于 2014-11-10 17:00:35 | 显示全部楼层
我现在遇到一个头疼的问题,电机码盘出来的脉冲不均匀,中间出现了连续7ms的高电平,可能是码盘坏了,测出来的速度没有任何意义,咋办啊
回复 支持 反对

使用道具 举报

发表于 2014-11-12 15:06:50 | 显示全部楼层
楼主啊,程序完整吗。有无更新哇
回复 支持 反对

使用道具 举报

发表于 2014-11-15 07:37:28 | 显示全部楼层
mark ..........
回复 支持 反对

使用道具 举报

发表于 2014-11-18 22:11:33 | 显示全部楼层
用你的程序小车顺利站起来,在这里对楼主表示感谢。{:soso_e183:}程序运行中发现两个问题一是:小车抖动中有时候会向一个方向扎过去而不是找到平衡后停止。二是:蓝牙控制,把小车放桌子上架起了测试前进后退还算管用,放到地上后看表面现象就是不管用。是不是pwm值设置的不够大?试着把值加大些实验结果一样,请问楼主如何解决?注:用的tt电机组不带测速电压是两节18650串联 uno+l298+mpu6050+蓝牙从机
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2024-3-19 18:21 , Processed in 0.061905 second(s), 29 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

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