Repository navigation
PID krmiljenje
Način sledenja črti s pomočjo PID krmiljenja. Uporabljenih je 8 senzorjev, ki so na spodnji sliki obkroženi z rdečo.
Znotraj void.setup() poteka kalibracija senzorjev za črto. Vedno pred sledenjem črti je potrebno te senzorje kalibrirati (bolj podroben opis kalibracije najdemo v primeru kalibracija).
Znotraj void.loop() pa je del programa za sledenje črti. Deluje tako, da s pomočjo funkcije robot.line.getPosition() preverja odmik črte od sredine robota in ga shranjuje v spremenljivko error. Izračuna se tudi integral (seštevek predhodnih odmikov omejen z minimalno in maksimalno vrednostjo) in difference (spremembo odmika izračunano iz prejšnjega in zdajšnjega odmika).
S pomočjo vrednosti error, integral in difference se izračuna vrednost spremenljivke rotate, ki opisuje zavijanje robota in je na koncu uporabljena v funkciji robot.drive(). Pri izračunu rotate vrednost error, integral in difference pomnožimo s pripadajočimi konstantami. Pravimo jim koeficient proporcionalnega ojačanja (kp), koeficient integralnega ojačanja (ki) in koeficient diferencialnega ojačanja (kd). Če je odmik majhen bo zavijanje rahlo, če je odmik velik pa bo zavijanje močno. Predznak odmika govori o tem v katero smer je potrebno zaviti.
#include <Robot.h>
int rotate = 0; //definicija spremenljivke, ki govori o rotaciji robota
float error = 0; //definicija spremenljivke, ki govori o napaki (odstopanju sredine robota od črte)
float integral = 0; //definicija spremenljivke, ki govori o integralu napake
float old_error = 0;
float difference = 0;
void setup() {
while (!robot.buttonPressed()); //čaka na pritisk tipke
delay(250); //250 ms
robot.motor[LEFT].setVoltage(1); //na levem motorju nastavi napetost 1 V
robot.motor[RIGHT].setVoltage(-1); //na desnem motorju nastavi napetost -1 V
robot.line.calibrate(2000); //kalibrira 2000ms
robot.motor[LEFT].setVoltage(0); //na levem motorju nastavi napetost 0 V
robot.motor[RIGHT].setVoltage(0); //na desnem motorju nastavi napetost 0 V
while (!robot.buttonPressed()); //čaka na pritisk tipke
delay(250);
}
void loop() {
error = robot.line.getPosition()*1000; //prebere stanje senzorjev in vrne lokacijo črte glede na robota [mm]
integral = constrain(integral + error, - 5, 5); //integral dobimo kot seštevek napak pri čemer ga omejimo med -5 in 5, da ne naraste preveč
difference = error - old_error;
old_error = error;
rotate = - 0.1 * error - 0.1 * integral - 0.1 * difference; //odmik, integral in spremembo odmika pomnožimo s svojimi konstantami -> večja vrednost rotate pomeni močnejše zavijanje
robot.drive(0.2, rotate); //vozi s hitrostjo 0.2 in izračunano rotacijo
}Program je v mnogih pogledih podoben tistemu v PI krmiljenju. Prav tako uporablja vseh 8 senzorjev, spremlja odmik od črte s funkcijo robot.line.getPosition() in za prilagajanje zavijanja spreminja spremenljivko rotate.
PID krmiljenje vsebuje tri konstante označene s kp, ki in kd. Njihov predznak govori o smeri zavijanja, velikost pa o načinu odziva na odmik robota od črte. P del krmiljenja je opisan že pod P krmiljenje, I del krmiljenja pa pod PI krmiljenje. Diferencialni del pa je pomemben, saj se v primeru hitre spremebe odmika močno spremeni tudi spremenljivka rotate. Z dodatkom tega dela h krmiljenju zmanjšamo nihanje robota. Prav tako to omogoči povečanje koeficienta kp, kar zagotovi hitrejši odziv. Več o teoriji PID krmiljenja lahko najdete tukaj.
