把速度环加入，PID双环原本是内环速度环外环直立环，带入传递公式，直立环的参数可以合并ka、ks成为一个大k，于是形式上成了两个环串联，公式如下：
-
小车有原始平衡角度，需要微调，但无需过度耗时间在这上面，精确到0.1就可以了，后面用速度环调节；
3.把速度环目标改成速度，小车就能跑起来。这里的速度目标与真实速度符号相反，是因为在测量速度时没有用双霍尔检测，只用了一个霍尔，如果用双霍尔，就得需要四个CHANGE中断，arduino单片机没有那么多，所以检测方向用了电机的AB使能端，这样只有在电机使能自我运转时才能获得速度值，不是真实值，方向是反的；
- 调节PID时记住口诀：
参数整定找最佳，从小到大顺序查。
先是比例后积分，最后再把微分加。
曲线振荡很频繁，比例度盘要放大。
曲线漂浮绕大湾，比例度盘往小扳。
曲线波动周期长，积分时间再加长。
曲线振荡频率快，先把微分降下来。
动差大来波动慢，微分时间应加长。
理想曲线两个波，前高后低4比1。
- 附全部代码：
#include <PinChangeInterrupt.h>
#include <MPU6050_tockn.h>
#include <Wire.h>
int STBY = 8; //使能端
int PWMA = 9; //右电机PWM输出控制脚
int AIN1 = 7; //右电机正极
int AIN2 = 6; //右电机负极
int PWMB = 10; //左电机PWM输出控制脚
int BIN1 = 13; //左电机正极
int BIN2 = 12; //左电机负极
#define PinA_left 4 //中断
#define PinA_right 2 //中断1
int count_right = 0,count_left = 0;
float leftSpeed = 0,rightSpeed = 0,speed = 0,angleX = 0;
int zeroAngle = -1; //小车初始平衡角度,需要微调
float angleKp = 11,angleKd = 0.4 ,anglePreError=0,angleError=0,angleOutput=0; //需要调整的角度环参数
float speedKp = 2.2, speedKi = 0.2 , speedTotalError=0,speedError=0,speedOutput=0; //需要调整的速度环参数
float speedTarget = 0; //负为向前，正为向后，绝对值不要超过3
float totalOutput=0;
unsigned long now=0,past=0,interval=50; //interval为小车调整姿态的时长,1000为一秒
MPU6050 mpu6050(Wire);
void setup(){
pinMode(STBY, OUTPUT);
pinMode(PWMA, OUTPUT);
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(PWMB, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
pinMode(PinA_left, INPUT); //测速码盘输入
pinMode(PinA_right, INPUT);
attachPinChangeInterrupt(digitalPinToPCINT(PinA_left), Code_left, CHANGE);
attachInterrupt(digitalPinToInterrupt(PinA_right), Code_right, CHANGE);
Serial.begin(9600);
Wire.begin();
mpu6050.begin();
mpu6050.calcGyroOffsets(true);
//MsTimer2::set(2000,getAngleX);
//MsTimer2::start();
}
void loop(){
now = millis();
if(now-past>interval){
getAngleX();
getSpeed();
angleOutput = calcAngleOutput(zeroAngle,angleX);
speedOutput = calcSpeedOutput(speedTarget,speed);
totalOutput = angleOutput-speedOutput;
Serial.print (angleOutput);
Serial.print ("|");
Serial.println(speedOutput);
Serial.print ("|");
Serial.println(totalOutput);
adjustPosture(totalOutput);
past = now;
}
}
void runset(int motor, int speed, int direction){
digitalWrite(STBY, 1);
if(motor==1 && direction== 1){
digitalWrite(AIN1,1);
digitalWrite(AIN2,0);
analogWrite(PWMA, speed);
}
if(motor==2 && direction== 1){
digitalWrite(BIN1,1);
digitalWrite(BIN2,0);
analogWrite(PWMB, speed);
}
if(motor==1 && direction== 0){
digitalWrite(AIN1,0);
digitalWrite(AIN2,1);
analogWrite(PWMA, speed);
}
if(motor==2 && direction== 0){
digitalWrite(BIN1,0);
digitalWrite(BIN2,1);
analogWrite(PWMB, speed);
}
}
void stop(){
digitalWrite(STBY, LOW);
}
void Code_left() {
count_left ++;
} //左测速码盘计数
void Code_right() {
count_right ++;
} //右测速码盘计数
void getSpeed()
{
if (digitalRead(AIN1)==1 && digitalRead(AIN2)==0){
rightSpeed = count_right;
}
else if (digitalRead(AIN1)==0 && digitalRead(AIN2)==1){
rightSpeed = -count_right;
}
if (digitalRead(BIN1)==1 && digitalRead(BIN2)==0){
leftSpeed = count_left;
}
else if (digitalRead(BIN1)==0 && digitalRead(BIN2)==1){
leftSpeed = -count_left;
}
count_left = 0;
count_right = 0;
//Serial.print(leftSpeed);
//Serial.print('\t');
//Serial.println(rightSpeed);
speed = (leftSpeed+rightSpeed)/2;
}
void getAngleX(){
mpu6050.update();
angleX = mpu6050.getAngleX();
}
float calcAngleOutput(float target,float pt){
angleError = target-pt;
angleOutput = angleKp*angleError + angleKd*(angleError-anglePreError);
anglePreError = angleError;
return angleOutput;
}
float calcSpeedOutput(float speedTarget,float pt){
speedError = speedTarget-pt;
speedTotalError += speedError;
speedTotalError = (abs(speedTotalError)>100)?0:speedTotalError;
speedOutput = speedKp*speedError + speedKi*speedTotalError;
return speedOutput;
}
void adjustPosture(float Output){
if (Output>0){
Output = (Output>254)?0:Output;
runset(1, (int)abs(Output), 1);
runset(2, (int)abs(Output), 1);
}
else{
Output = (Output<-254)?0:Output;
runset(1, (int)abs(Output), 0);
runset(2, (int)abs(Output), 0);
}
}
