Skocz do zawartości

[C] Problem z profilerem prędkości (PID)


Marcin_56

Pomocna odpowiedź

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:

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 int	kdX=2;

short int 	regulacjaW;
short int   kpW=1;
short int	kdW=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 int	PWMX = 0;
short int	PWMW = 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
}
Link do komentarza
Share on other sites

Bądź aktywny - zaloguj się lub utwórz konto!

Tylko zarejestrowani użytkownicy mogą komentować zawartość tej strony

Utwórz konto w ~20 sekund!

Zarejestruj nowe konto, to proste!

Zarejestruj się »

Zaloguj się

Posiadasz własne konto? Użyj go!

Zaloguj się »
×
×
  • Utwórz nowe...

Ważne informacje

Ta strona używa ciasteczek (cookies), dzięki którym może działać lepiej. Więcej na ten temat znajdziesz w Polityce Prywatności.