Problem z profilerem prędkości (PID)

2017-03-02T17:39:00+01:00

Cześć ,

robię profiler prędkości(PID) do robota dwukołowego. Mam problem z wyliczeniem przejechanej trasy, tak aby robot przejechał dany kawałek po PROSTEJ i się zatrzymał. Umieszczam na dole kod z zaznaczonym,i miejscami gdzie mam problem. Wiem mniej więcej co ma tam być ale robot nie działa poprawnie. Czy byłby ktoś wstanie mi pomóc w kwestii prędkości/trasy do tego kodu ?

Silniki są sterowane PWM
Średnica kół to 3.22 cm.
Rozstaw kół to 90 mm.
Ilość "tików" enkodera na obrót koła to 300

Wiem, że jest ogromny chaos z zmiennymi i tym gdzie powinny być(globalne/lokalne, rozmiar ich), ale to już sobie poprawię jak całość będzie działać
Kod:
[syntax=c]int docelowaPredkoscW = 100; //przykladowe
int docelowaPredkoscX = 500; //przykladowe

short int encX;
short int encW;

short int dccX = 2;
short int dccW = 1;

short int aktualnaPredkoscX = 0;// aktualnie zadana prędkość
short int aktualnaPredkoscW = 0;// aktualnie zadana prędkość

short int regulacjaX;
short int kpX=1;
short intkdX=2;

short int regulacjaW;
short int kpW=1;
short intkdW=1;

volatile int encoder_R;
volatile int encoder_L;

short int lewyEnc=0;
short int prawyEnc=0;
short int zmianaEncL=0;
short int zmianaEncP=0;
short int lewyEncStary=0;
short int prawyEncStary=0;

void Enkodery(void)
{
prawyEnc = encoder_R; // aktualna wartosc z prawego enkoder
lewyEnc = encoder_L; // aktualna wartosc z lewego enkoder

//rozniczka z polozenia -> predkosc

zmianaEncL = lewyEnc - lewyEncStary;
zmianaEncP = prawyEnc - prawyEncStary;

lewyEncStary = lewyEnc;
prawyEncStary = prawyEnc;

//predkosc w ruchu translacji i translacji
encX = (zmianaEncP + zmianaEncL);
encW = (zmianaEncP - zmianaEncL);
}

void AktualnaPredkosc()
{
//translacja
if( ) // <===== // nie wiem co Tu wpisac //TODO// <===== // nie wiem co Tu wpisac //TODO
{
docelowaPredkoscX = 0;
}

if( aktualnaPredkoscX < docelowaPredkoscX )
{
aktualnaPredkoscX += dccX;
if(aktualnaPredkoscX > docelowaPredkoscX)
{
aktualnaPredkoscX = docelowaPredkoscX;
}
}

else if( aktualnaPredkoscX > docelowaPredkoscX )
{
aktualnaPredkoscX -= dccX;
if( aktualnaPredkoscX < docelowaPredkoscX )
{
aktualnaPredkoscX = docelowaPredkoscX;
}
}

//rotacja
if( ) // <===== // nie wiem co Tu wpisac //TODO
{
docelowaPredkoscW = 0;
}

if(aktualnaPredkoscW < docelowaPredkoscW)
{
aktualnaPredkoscW += dccW;
if(aktualnaPredkoscW > docelowaPredkoscW)
{
aktualnaPredkoscW = docelowaPredkoscW;
}
}

else if(aktualnaPredkoscW > docelowaPredkoscW)
{
aktualnaPredkoscW -= dccW;
if(aktualnaPredkoscW < docelowaPredkoscW)
{
aktualnaPredkoscW = docelowaPredkoscW;
}
}
}

void odchylenieX()
{
regulacjaX = aktualnaPredkoscX - encX;
}

void odchylenieW()
{
regulacjaW = aktualnaPredkoscW - encX;

//tutaj kod do ustalanie blędów
// uwzgledniajac czyjniki (lewy i prawy)
}

short int regulacjaXStara = 0;
short int regulacjaWStara = 0;

void ComputePID()
{
short intPWMX = 0;
short intPWMW = 0;

Enkodery(); //funkcja dotyczaca enkoderow, zmieranie z nich danych

AktualnaPredkosc(); // aktualizacja prednkosci

odchylenieW();
odchylenieX();

PWMX = (kpX * regulacjaX) + (kdX * (regulacjaX - regulacjaXStara)); //regulator PD translacji
PWMW = (kpW * regulacjaW) + (kdW * (regulacjaW - regulacjaWStara)); //regulator PD rotacji

regulacjaXStara = regulacjaX;
regulacjaWStara = regulacjaW;

Left_Motor(PWMX - PWMW); //sterowaniem lewym sinikiem za pomocą PWM
Right_Motor(PWMX + PWMW); //sterowaniem prawym sinikiem za pomocą PWM
}[/syntax]

Statystyki: Napisane przez Marcin_5656 — 2 mar 2017, o 17:39

ATNEL tech-forum

Problem z profilerem prędkości (PID)