void __attribute__((__interrupt__)) _T1Interrupt(void)
{
long d, t;
long sint, cost;
long x, y;
long greska;
int brzina, commande_distance, commande_rotation;
sys_time++;
if (++brint == 5)
{
brint = 0;
// **********************************************************************
// ODOMETRIJA (ide na 5ms)
// **********************************************************************
//CITANJE PODATAKA O POZICIJAMA OBA MOTORA:
vR = -(int)POS2CNT; //ocitavamo broj inkremenata u zadnjoj periodi
POS2CNT = 0; //resetujemo brojac inkremenata
positionR += vR; //azuriramo trenutnu poziciju
vL = (int)POS1CNT; //ocitavamo broj inkremenata u zadnjoj periodi
POS1CNT = 0; //resetujemo brojac inkremenata
positionL += vL; //azuriramo trenutnu poziciju
L = (positionR + positionL) / 2;
orientation = (positionR - positionL) % K1;
if(orientation > K1/2)
orientation -= K1;
if(orientation < -K1/2)
orientation += K1;
teta = (orientation * 16384) / (K1 / 2);
if (teta < 0)
teta += 32768;
d = vR + vL; //ovo se kasnije deli sa 2
if(teta < 8192)
{
t = teta;
sint = sinus[t];
cost = sinus[8191 - t];
}
else
if (teta < 16384)
{
t = teta - 8192;
sint = sinus[8191 - t];
cost = -sinus[t];
}
else
if (teta < 24576)
{
t = teta - 16384;
sint = -sinus[t];
cost = -sinus[8191 - t];
}
else
{
t = teta - 24576;
sint = -sinus[8191 - t];
cost = sinus[t];
}
x = d * cost;
y = d * sint;
Xlong += x;
Ylong += y;
X = ((long long)Xlong) >> 16;
X /= K2;
Y = ((long long)Ylong) >> 16;
Y /= K2;
//regulator rastojanja
brzina = (vL + vR) / 2;
greska = d_ref - L;
zaglavL = greska >= 0 ? greska : -greska;
commande_distance = greska * Gp_D - Gd_D * brzina;
//regulator orjentacije
brzina = vL - vR;
greska = (orientation - t_ref) % K1;
if (greska > K1/2)
greska -= K1;
if (greska < -K1/2)
greska += K1;
zaglavR = greska >= 0 ? greska : -greska;
commande_rotation = greska * Gp_T - brzina * Gd_T;
PWML = commande_distance - commande_rotation;
PWMD = commande_distance + commande_rotation;
// saturacija
if(PWMD <= -3200)
PWMD = -3200;
else if(PWMD >= 3200)
PWMD = 3200;
if(PWML <= -3200)
PWML = -3200;
else if(PWML >= 3200)
PWML = 3200;
//izbor smera:
if(PWML >= 0)
{
LEVI_NAPRED;
LeviPWM(PWML);
}
else
{
LEVI_NAZAD;
LeviPWM(-PWML);
}
if (PWMD >= 0)
{
DESNI_NAPRED;
DesniPWM(PWMD);
}
else
{
DESNI_NAZAD;
DesniPWM(-PWMD);
}
} //KRAJ REGULACIJE
void __attribute__((__interrupt__, auto_psv)) _T1Interrupt(void)
{
sys_time++;
//if(++brInt == 3)
// {
// brInt = 0;
//CITANJE PODATAKA O POZICIJAMA OBA MOTORA:
vR = (int)POS2CNT; //ocitavamo broj inkremenata u zadnjoj periodi
POS2CNT = 0; //resetujemo brojac inkremenata
positionR += vR; //azuriramo trenutnu poziciju
vL = -(int)POS1CNT;
POS1CNT = 0; //resetujemo brojac inkremenata
positionL += vL; //azuriramo trenutnu poziciju
L = (positionR + positionL) / 2;
orientation = (positionR - positionL) % K1;
if(orientation > K1/2)
orientation -= K1;
if(orientation < -K1/2)
orientation += K1;
//ovde je izbegnuta prva long long operacija
/*PRE*/// teta = (long long) orientation * 32768 / K1;
teta = (orientation * 16384) / (K1 / 2);
// 32768-> pun krug, podeljeno sa K1 daje neke kvazi jedinice u zavisnosti
// od parametara sistema, rezolucije enkodera, tockova, rastojanja...
if(teta < 0)
teta += 32768;
// tabela sinusa ima 8192 elemenata -> prvi kvadrant
d = vR + vL; //DODATO DELJENJE!!!
//d = vR + vL;
// uvek se svodi na prvi kvadrant!
if(teta < 8192)
{
t = teta;
sint = sinus[t];
cost = sinus[8191 - t];
}
else
if(teta < 16384) // DRUGI KVADRANT
{
t = teta - 8192;
sint = sinus[8191 - t];
cost = -sinus[t];
}
else
if(teta < 24576) // treci kvadrant
{
t = teta - 16384;
sint = -sinus[t];
cost = -sinus[8191 - t];
}
else // 4. kvadrant
{
t = teta - 24576;
sint = -sinus[8191 - t];
cost = sinus[t];
}
// x, y -> predjeno po x i y koordinati u zadnjoj periodi (deltax, deltay)
x = d * cost;
y = d * sint;
// x, y je min 32767
Xlong += x;
Ylong += y;
//izacunavanje trenutne pozicije [mm]
// pre:
//X = (((long long)Xlong / 2) / 32768) / K2;
//Y = (((long long)Ylong / 2) / 32768) / K2;
// pretvaranje u inkremente
X = (((long long)Xlong) / 32768) / 2;
// pretvaranje u milimetre
X /= K2;
Y = (((long long)Ylong) / 32768) / 2;
Y /= K2;
//regulator rastojanja
brzina = (vL + vR) / 2; // srednja vrednost predjenih inkremenata u zadnjoj periodi -> v = s/t, brzina
greska = d_ref - L;
zaglavL = (greska >= 0 ? greska : -greska);
commande_distance = greska * Gp_D - Gd_D * brzina;
//regulator orjentacije
brzina = vL - vR;
greska = (orientation - t_ref) % K1;
if(greska > K1/2)
greska -= K1;
if(greska < -K1/2)
greska += K1;
zaglavR = (greska >= 0 ? greska : -greska);
commande_rotation = greska * Gp_T - brzina * Gd_T;
// ukupan PWM dobija se superpozicijom uzimajuci u obzir oba regulatora
// commande_distance = regulator rastojanja
// commande_rotation = regulator orijentacije
PWML = commande_distance - commande_rotation;
PWMD = commande_distance + commande_rotation;
// saturacija
if(PWMD <= -3200)
PWMD = -3200;
else if(PWMD >= 3200)
PWMD = 3200;
if(PWML <= -3200)
PWML = -3200;
else if(PWML >= 3200)
PWML = 3200;
//izbor smera:
if(PWML >= 0)
{
LATBbits.LATB9 = 0;
LeviPWM(PWML);
}
else
{
LATBbits.LATB9 = 1;
LeviPWM(3200 + PWML);
}
if (PWMD >= 0)
{
LATBbits.LATB15 = 0;
DesniPWM(PWMD);
}
else
{
LATBbits.LATB15 = 1;
DesniPWM(3200 + PWMD);
}
// } //KRAJ REGULACIJE
//}
IFS0bits.T1IF = 0; /* Clear Timer interrupt flag */
}
