关于平衡小车速度环系数整定
我用的板子是UNO、陀螺仪是串口直接输出角度的MPU6050（gy250)、电机驱动用的普通L298n、电机也是垃圾的香蕉电机、测速模块是光电对射测码盘的。本帖最后由 ILLUSION 于 2016-10-21 09:34 编辑
下面贴上代码并附上直立换调试后的效果
#include <L298.h>
#include <Wire.h>
#include <PID_v1.h>
//电机控制
L298 motor(5,4,6,7);
double output,output_left,output_right;
//角度读取
float YPR;
unsigned char Re_buf,counter=0;
unsigned char sign=0;
//速度位置控制
float Ksp = 20,Ksi = 0.1; //速度环 ki kp
float shudu_weizhi_out;
int EncoderImpulse_left,EncoderImpulse_right; //左右编码器脉冲数
int v_left,v_right,weizhi; //左右速度、小车位置
//直立控制
double angle_set=-0.8,angle_input,angle_output,angle_kp=60,angle_ki=0,angle_kd=10.5; //设定平衡角度，KP\ki\kd值
PID myPID(&angle_input, &angle_output, &angle_set, angle_kp, angle_ki,angle_kd, DIRECT);
//转向控制
//未完成
void setup()
{
Serial.begin(115200); //初始化
pinMode(4,INPUT);
pinMode(7,INPUT);
attachInterrupt(0,impulse_left, FALLING); //中断
attachInterrupt(1,impulse_right, FALLING);
Serial.write(0XA5);
Serial.write(0X52); //初始化GY25,连续输出模式
//myPID.SetSetpoint(angle_set);
//myPID.SetTunings(angle_kp,angle_ki, angle_kd);
myPID.SetSampleTime(10);
myPID.SetOutputLimits(-255,255);
myPID.SetMode(AUTOMATIC);
}
//-------------------------------------------------------------
void loop()
{
if(sign)
{
sign=0;
angle_input =YPR; // 输入赋值，即为测得当前角度
myPID.Compute();
speed_clc();
shudu_weizhi_out = (v_left+v_right)/2*Ksp + weizhi*Ksi;
output=angle_output+shudu_weizhi_out;
output_left=output;
output_right=output;
if(abs(angle_input)<15) //角度大于15度即停止输出
{
motor.MotorA(output_left);
motor.MotorB(output_right);
}
else
{
motor.MotorA(0);
motor.MotorB(0);
}
Serial.print(angle_input); // 计算时间
Serial.print(",");
Serial.print(v_left); // 计算时间
Serial.print(",");
Serial.print(v_right); // 计算时间
Serial.print(",");
Serial.print(angle_output); // 计算时间
Serial.print(",");
Serial.print(shudu_weizhi_out); // 计算时间
Serial.print(",");
Serial.println(output); // PID输出值
// 记录本次时间
}
}
void serialEvent() { //MPU6050代码
while (Serial.available()) {
Re_buf=(unsigned char)Serial.read();
if(counter==0&&Re_buf!=0xAA) return; // 检查帧头
counter++;
if(counter==8) //接收到数据
{
counter=0; //重新赋值，准备下一帧数据的接收
if(Re_buf==0xAA && Re_buf==0x55) //检查帧头，帧尾
{
YPR=(Re_buf<<8|Re_buf)/100.0; //合成数据，去掉小数点后2位
YPR=(Re_buf<<8|Re_buf)/100.0;
YPR=(Re_buf<<8|Re_buf)/100.0;
}
sign=1;
}
}
}
void impulse_left(void) //测左轮脉冲数
{
if(digitalRead(4)==0)EncoderImpulse_left =EncoderImpulse_left-1; //读取数字口4电平状态，若是低电平，代表轮子正传。脉冲数自加
else EncoderImpulse_left = EncoderImpulse_left+1; //若是高电平，代表轮子反转。脉冲数自减
}
void impulse_right(void) //测右轮脉冲数
{
if(digitalRead(7)==1)EncoderImpulse_right = EncoderImpulse_right-1; //读取数字口4电平状态，若是低电平，代表轮子正传。脉冲数自加
else EncoderImpulse_right = EncoderImpulse_right+1; //若是高电平，代表轮子反转。脉冲数自减
}
void speed_clc(void)
{
static int speed_time_old,EncoderImpulse_left_old,EncoderImpulse_right_old;
if(millis() - speed_time_old >= 100)
{
v_left= EncoderImpulse_left-EncoderImpulse_left_old;
v_right = EncoderImpulse_right -EncoderImpulse_right_old;
weizhi =( EncoderImpulse_left+EncoderImpulse_right)/2;
//这个地方可以对上面的俩数度和位置滤波
EncoderImpulse_left_old= EncoderImpulse_left;
EncoderImpulse_right_old = EncoderImpulse_right;
speed_time_old =millis();
}
} 本帖最后由 ILLUSION 于 2016-10-21 10:02 编辑
速度还一直整定的不好，不像别人的那样平衡的时候，推一下会自动调节，并且回到原来位置。请大神帮忙看看代码有什么不对的地方
直立视频地址 http://v.youku.com/v_show/id_XMTc2ODM2NzIwNA==.html
页:
[1]