想做PID控制马达的速度

coolcxm

arduino 328+6050+298N做了个平衡车，立起来是很稳 ，一加上速度环就出问题。无法控制前进后退，要不就是让前进不前进，要不就是越跑越快然后倒下，要不就是点停止时，自己又跑。

一直没明白速度环代码。有谁能给一段例子代码啊，我现在小车是通过定时器，10ms调一次pid的。代码是抄的

感觉这速度环部份的代码是不是有错啊。但自己微积分全忘光了。百度找来的公式全看不明白。都不知道怎么调了。

void  PIDD() {

        position_dot = (countA + countB)*0.5;               
        position_dot_filter *= 0.8;               
        position_dot_filter += position_dot*0.2;
        positiono += position_dot_filter;         
        positiono += Speed_Need;
        positiono = constrain(positiono, -800, +800);

        Output = angle *kap + angle_dot *kai + positiono*ksp + position_dot_filter *ksi;  
}

单片机菜鸟

楼主最终搞定了吗  看到很多人都在玩平衡车

coolcxm

还没搞出来啊。只能平衡不能行走

ILLUSION

看着代码没问题吖，关掉直立环调试速度环，速度是怎么反馈的，编码器？

coolcxm

ILLUSION 发表于 2016-12-16 09:23
看着代码没问题吖，关掉直立环调试速度环，速度是怎么反馈的，编码器？

是用编码器，关掉直立环。，用手拿起车子，设KSP设为2或3时，ksi=0然后控制 Speed_Need为100这时候，前进后退是正常的。
然后再设Speed_Need为停，轮会慢慢停，然后前后摆动到静止，加上KSI就可以基本上稳定的停下来了。但加上直立环之后。KSP就不能设成2或3了，要设一个很小的数，这样，前进后退也不能行走了。而且加上了 ksp ksi。开启直立环，车子会动，很难稳住在一个位置，只有设ksp ksi设0才能稳


代码是抄 http://www.geek-workshop.com/for ... =%C6%BD%BA%E2%B3%B5
的，只是将4个电阻去除，用用数据进去，马达减速比1:30，390线的编码器。
轮子直径65mm，马达扭力 1kgf.cm

ILLUSION

coolcxm 发表于 2016-12-16 09:32
是用编码器，关掉直立环。，用手拿起车子，设KSP设为2或3时，ksi=0然后控制 Speed_Need为100这时候，前 ...

又看了下代码，你的 position_dot = (countA + countB)*0.5里面。count确定是每多少毫秒的值吧？而不是编码器直接输出值吧？
还有这个 positiono += Speed_Need;，对位置积分是个啥？

yusanfengyi

我先表达一下看法，不知道说的对不对，不对的大家可以指出来，首先，L298N是控制步进电机吗？如果你控制步进电机，那么，脉冲数量决定着位移，频率决定着速度，这句话对吧？
那么来了，你既然控制速度，就得涉及到调频，据我了解，如果你使用ARDUINO默认的频率恒定的话，你是可以干别的事情的，假如你自己写了一段定时调频代码，如下所示：
int PUL=4;
int DIR=3;
int K0=0;

void setup()
{  
    pinMode(DIR,OUTPUT);
    pinMode(PUL,OUTPUT);
    pinMode(K0,INPUT);
}

void loop()
{
  digitalWrite(K0,HIGH);
  if (digitalRead(K0)==HIGH)
  {   
    digitalWrite(DIR,LOW);
    unsigned int number=1000; unsigned int time=100;
  
    while (number>0)
    {
           digitalWrite(PUL,HIGH);
           delayMicroseconds(time);
           digitalWrite(PUL,LOW);
           delayMicroseconds(time);
     }
      number--;
  }
}

此时你的CPU什么也干不了，只能处理这一件事情，要么就卡死，这就是ARDUINO调频的缺点。
会不会是这样的原因呢？

coolcxm

yusanfengyi 发表于 2016-12-18 13:09
我先表达一下看法，不知道说的对不对，不对的大家可以指出来，首先，L298N是控制步进电机吗？如果你控制步 ...

那程序是用定时器中断的

coolcxm

ILLUSION 发表于 2016-12-18 09:39
又看了下代码，你的 position_dot = (countA + countB)*0.5里面。count确定是每多少毫秒的值吧？而不是 ...

我就是没明白啊，看不懂这段

COUNT是编码器的值 ，编码器会一直计数，调完pwm后，直接清零。

coolcxm

我将原作者的代码贴上来吧
#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)));
}

我只是将4个可调电阻用程序值来代替了。

ILLUSION

coolcxm 发表于 2016-12-19 21:06
我就是没明白啊，看不懂这段

COUNT是编码器的值 ，编码器会一直计数，调完pwm后，直接清零。

程序count哪里有清零了？我之前弄得时候，是定义了 every 10ms  ; float speedA =countA - countA_old;countA_old = countA。这样才是测每10ms的速度。不过用的码盘，分辨率不够，所以效果也不好。要是你的分辨率也不够的话就别用速度滤波了，注释掉看看。

coolcxm

ILLUSION 发表于 2016-12-20 09:41
程序count哪里有清零了？我之前弄得时候，是定义了 every 10ms  ; float speedA =countA - countA_old;co ...

void flash()
{
ADOM();
PIDD();
countA=countB=0;
}
这段不是清零了吗

flash()是用计时器操作的啊

coolcxm

coolcxm 发表于 2016-12-20 11:32
void flash()
{
ADOM();

换了个马达驱动 TB6612FNG
感觉 298N马达有点肉。TB6612FNG反应速度会好一些的感觉。但程序上速度环这块还是不明白啊，能不能解释一下啊

还有，是不是少了左右轮差速调整啊。2轮速度不太一至的感觉

加上了速度环，动一下就跑了。
如果没加还好很多。