使用二路紅外線避障模組來做循線行進。並利用PID控制來增進控制的穩定度和流暢度。但結果沒有調整到很理想的狀態。我認為有以下幾個原因：
- 馬達速度控制不精確
雖然有事先測量過使兩邊的轉速差不多，但因為沒有用輪子上紅外線的速度回饋，所以誤差還是存在，進而影響到PID的穩定性。
- 避障感測器不足
在遇到弧度較大的彎曲時，如果還來不及修正就讓兩個避障模組偵測到白色，以我的程式的算法就會認定成走在直線上而出軌，因此增加兩路變成四路就能有更長的修正時間就能解決問題。當然改一下感測器的輸入算法或繼續調整PID的參數也是可以，但我懶。
應該還有其他原因，但我目前想不到，就先這樣。
程式碼：
#include <AFMotor.h>
AF_DCMotor motorLeft(2);
AF_DCMotor motorRight(3);
int DigitalPin1 = A5;
int DigitalPin2 = A2;
int cnt1 = 0;
int last1 = 0;
int cnt2 = 0;
int last2 = 0;
unsigned long timer;
unsigned long dt;
float rpm;
float power;
float intergral_bias[2], last_bias[2];
const int init_speed = 150;
const float Kp = 3.3;
const float Ki = 0.002;
const float Kd = 2.2;
int PID_motor(int rpm,int target,int i){
float bias = -1*(rpm-target);
intergral_bias[i] = intergral_bias[i]*0.7 + bias;
float power = Kp*bias + Ki*intergral_bias[i] + -1*Kd*(bias-last_bias[i])+init_speed;
if(power > 255){
power = 255;
}
if(power < 10){
power = 10;
}
last_bias[i] = bias;
Serial.println(power);
return power;
}
void setup()
{
Serial.begin(115200);
pinMode(DigitalPin1, INPUT);
pinMode(DigitalPin2, INPUT);
motorLeft.setSpeed(init_speed);
motorLeft.run(FORWARD);
//motorRight.setSpeed(init_speed);
//motorRight.run(FORWARD);
}
void loop()
{
timer = millis();
//Serial.println(timer);
while(cnt1 < 10){
boolean D_val1 = digitalRead(DigitalPin1);
//boolean D_val2 = digitalRead(DigitalPin2);
if(abs(last1 - D_val1) == 1){
cnt1++;
}
//if(abs(last2 - D_val2) == 2){
// cnt2++;
//}
last1 = D_val1;
//last2 = D_val2;
}
dt = millis() - timer;
rpm = (60000/float(dt));
//Serial.println("rpm= ");
Serial.print(rpm);
Serial.print(" ");
cnt1 = 0;
power = PID_motor(rpm,80,0);
motorRight.setSpeed(power);
}影片：