Output=Kp*Angle+Kd*Gyro_y+ Ksp*speeds_filter + Ksi*positions ; 里面的KsiKsp 是什么参数 没有用到I吗？
这是用PD控制的对吗？
Processing 图形工具在线调试PID参数教程 这个网址打不开，楼主能给个教程不？？
smallfivecn 发表于 2014-8-29 10:15 static/image/common/back.gif
楼主：你的电机测速为何只接了一路呀？每个电机测速都有A、B相输出，接两路不是更准确吗？我用的是TB6612电 ...
恩恩 我也用AB测速但不知道怎么写代码
我的还无法站立。。。
楼主,关于.Processing 图形工具在线调试PID的文章已经找不到了,现在我的小车调试调不明白,希望楼主能给些意见和资料让我参考一下,非常感谢你.
感谢分享 :D
楼主你好，你的pin2和pin3接的是什么输入，暂时搞不懂呀
{:soso_e179:}
我的怎么也站不起来！
#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)));
}
lz，有没有更加详细的接线图？以及那个12V的输入是用的电池咩？
楼主牛人啊，好资源果断收藏！
我现在遇到一个头疼的问题，电机码盘出来的脉冲不均匀，中间出现了连续7ms的高电平，可能是码盘坏了，测出来的速度没有任何意义，咋办啊
楼主啊，程序完整吗。有无更新哇
mark ..........
用你的程序小车顺利站起来，在这里对楼主表示感谢。{:soso_e183:}程序运行中发现两个问题一是：小车抖动中有时候会向一个方向扎过去而不是找到平衡后停止。二是：蓝牙控制，把小车放桌子上架起了测试前进后退还算管用，放到地上后看表面现象就是不管用。是不是pwm值设置的不够大？试着把值加大些实验结果一样，请问楼主如何解决？注：用的tt电机组不带测速电压是两节18650串联 uno+l298+mpu6050+蓝牙从机