Ejemplo n.º 1
0
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
Ejemplo n.º 2
0
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 */
}