想做PID控制马达的速度
本帖最后由 coolcxm 于 2016-12-14 21:15 编辑arduino 328+6050+298N做了个平衡车，立起来是很稳 ，一加上速度环就出问题。无法控制前进后退，要不就是让前进不前进，要不就是越跑越快然后倒下，要不就是点停止时，自己又跑。
一直没明白速度环代码。有谁能给一段例子代码啊，我现在小车是通过定时器，10ms调一次pid的。代码是抄的
感觉这速度环部份的代码是不是有错啊。但自己微积分全忘光了。百度找来的公式全看不明白。都不知道怎么调了。
voidPIDD() {
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 于 2016-12-16 09:37 编辑
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/forum.php?mod=viewthread&tid=8972&highlight=%C6%BD%BA%E2%B3%B5
的，只是将4个电阻去除，用用数据进去，马达减速比1:30，390线的编码器。
轮子直径65mm，马达扭力 1kgf.cm 本帖最后由 ILLUSION 于 2016-12-18 09:41 编辑
coolcxm 发表于 2016-12-16 09:32
是用编码器，关掉直立环。，用手拿起车子，设KSP设为2或3时，ksi=0然后控制 Speed_Need为100这时候，前 ...
又看了下代码，你的 position_dot = (countA + countB)*0.5里面。count确定是每多少毫秒的值吧？而不是编码器直接输出值吧？
还有这个 positiono += Speed_Need;，对位置积分是个啥？ 我先表达一下看法，不知道说的对不对，不对的大家可以指出来，首先，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调频的缺点。
会不会是这样的原因呢？ yusanfengyi 发表于 2016-12-18 13:09
我先表达一下看法，不知道说的对不对，不对的大家可以指出来，首先，L298N是控制步进电机吗？如果你控制步 ...
那程序是用定时器中断的 本帖最后由 coolcxm 于 2016-12-19 21:09 编辑
ILLUSION 发表于 2016-12-18 09:39
又看了下代码，你的 position_dot = (countA + countB)*0.5里面。count确定是每多少毫秒的值吧？而不是 ...
我就是没明白啊，看不懂这段
COUNT是编码器的值 ，编码器会一直计数，调完pwm后，直接清零。 我将原作者的代码贴上来吧
#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 = {{ 1, 0 },{ 0, 1 }};
static float Pdot ={ 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=Q_angle - P - P;
Pdot=- P;
Pdot=- P;
Pdot=Q_gyro;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
angle_err = angle_m - angle;
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;
}
voidPIDD(){
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个可调电阻用程序值来代替了。
coolcxm 发表于 2016-12-19 21:06
我就是没明白啊，看不懂这段
COUNT是编码器的值 ，编码器会一直计数，调完pwm后，直接清零。
程序count哪里有清零了？我之前弄得时候，是定义了 every 10ms; float speedA =countA - countA_old;countA_old = countA。这样才是测每10ms的速度。不过用的码盘，分辨率不够，所以效果也不好。要是你的分辨率也不够的话就别用速度滤波了，注释掉看看。 ILLUSION 发表于 2016-12-20 09:41
程序count哪里有清零了？我之前弄得时候，是定义了 every 10ms; float speedA =countA - countA_old;co ...
void flash()
{
ADOM();
PIDD();
countA=countB=0;
}
这段不是清零了吗
flash()是用计时器操作的啊 本帖最后由 coolcxm 于 2016-12-20 17:53 编辑
coolcxm 发表于 2016-12-20 11:32
void flash()
{
ADOM();
换了个马达驱动 TB6612FNG
感觉 298N马达有点肉。TB6612FNG反应速度会好一些的感觉。但程序上速度环这块还是不明白啊，能不能解释一下啊
还有，是不是少了左右轮差速调整啊。2轮速度不太一至的感觉
加上了速度环，动一下就跑了。
如果没加还好很多。
页:
[1]