Witam.
Stworzyłem ostatnio swojego pierwszego robota typu LineFollower. Konstrukcję mechaniczną wraz z całą elektroniką mam ukończoną, pozostało jedynie napisać odpowiedni program. Przy budowie robota posiłkowałem się wieloma poradnikami dostępnymi w internecie stąd od początku moim celem było zastosowanie regulatora PD do podążania za trasą. I tu pojawia się problem gdyż już któryś dzień z rzędu staram się dobrać odpowiednie nastawy i wszelkie próby kończą się fiaskiem. Dlatego proszę o pomoc na forum, gdzie na pewno znajdują się ludzie, którzy mają doświadczenie w tej kwestii.
Procesor użyty to Atmega 328p, program pisany jest w języku C z użyciem Arduino IDE. Robot posiada 8 czujników umieszczonych w lekkim półkolu w odległości ~14mm od siebie.
Regulator staram się stroić wyłączając część D i próbując dobrać odpowiednie Kp tak aby robot był w stanie pokonywać zakręty (gdzieś czytałem, że podobno sam człon P wystarczy aby przejechać trasę ale nie mam pojęcia ile w tym prawdy). Problem jest jednak taki, że przy małych wartościach Kp robot wypada z trasy na bardzo łagodnych zakrętach (około 30 stopni) a przy większych z trudem dojeżdża do zakrętu, gdyż na prostej dość mocno się chwieje. Włączenie części różniczkującej w moim odczuciu niewiele daje (nie widać jakiejś zauważalnej poprawy).
Być może jest to wina tego, że przy wykryciu skrętu część różniczkująca jest różna od zera tylko i wyłącznie pomiędzy samplami w których wykryto różnicę w odczycie - poźniej, pomimo wykrywania zakrętu odczyt pozostaje taki sam wobec czego różniczka znów jest zerem co sprawia, że część D była obecna w sygnale sterującym silnikami tylko przez ~5ms (= brak reakcji silników). Z drugiej strony jednak, wszystkie przykładowe kody bądź pseudokody regulatora PD dla LineFollowera wyglądały tak samo, więc nie wiem czy moja hipoteza jest prawdziwa (pewnie nie).
Poniżej zamieszczam kod. Nie jest to całkowity kod programu, wkleiłem tylko to co jest istotne z punktu widzenia regulatora PD.
const int SensorWeight[8] = { -8, -4, -2, -1, 1, 2, 4, 8};
const int SetPoint = 0;
const int NoActiveSensor = 221;
int Kp = 1;
int Td = 0;
int gain = 0;
int derivative = 0;
int feedback = 0;
int control = 0;
int baseMotorSpeed = 50;
unsigned long int startTime;
int GetSensorValues()
{
int value = 0;
int activeSensors = 0;
for (int i = 0; i < 8; i++)
{
if (digitalRead(i))
{
value += SensorWeight[i];
activeSensors++;
}
}
if(!activeSensors)
return NoActiveSensor;
value = value / activeSensors;
return value;
}
void SetMotors(int speed)
{
SetLeftMotor(baseMotorSpeed + speed);
SetRightMotor(baseMotorSpeed - speed);
}
void SetLeftMotor(int speedValue)
{
if (speedValue<0)
speedValue = 0;
else if(speedValue>255)
speedValue = 255;
analogWrite(IA1, speedValue);
}
void SetRightMotor(int speedValue)
{
if (speedValue<0)
speedValue = 0;
else if(speedValue>255)
speedValue = 255;
analogWrite(IB1, speedValue);
}
void PD() {
startTime = millis();
feedback = GetSensorValues();
if(feedback == NoActiveSensor)
error = previousError;
else
error = SetPoint - feedback;
gain = Kp * error;
derivative = (error - previousError) * Td;
control = gain + derivative;
SetMotors(control);
previousError = error;
while (millis() - startTime < 5);
}
Będę wdzięczny za pomoc i z góry dziękuję.