eMotor Phones Home

The encoder readouts depend on special modules provided in the dsPIC MC (Motor Control) family. Before we begin, I advise that you download Measuring Speed and Position with the QEI Module by Microchip. I just read the article and copy the code in there (and found that there were some minor typos such as missing parentheses, but they were easy to spot.)

Since the initializtaion, position and speed calculation in my program are almost the same as shown in that article, there’s no need to explain the routines below. I only adjust some variables to match the encoder lines and maximum velocity of the eMotor. The resulting eMotor speed is kept in variables Veli and Velf. The former contains the raw encoder count (x 4) difference between sampling period, and the latter contains the conversion to revolution per sec unit. The rotation direction is also adjusted properly by checking the UPDN bit of QEICON register.

// ----------- quadrature encoder functions ------------------------
void InitQEI(void)
{
    ADPCFG |= 0x0038; // Configure QEI pins as digital inputs
    QEICONbits.QEIM = 0; // Disable QEI Module
    QEICONbits.CNTERR = 0; // Clear any count errors
    QEICONbits.QEISIDL = 0; // Continue operation during sleep
    QEICONbits.SWPAB = 0; // QEA and QEB not swapped
    QEICONbits.PCDOUT = 0; // Normal I/O pin operation
    QEICONbits.POSRES = 1; // Index pulse resets position counter
    DFLTCONbits.CEID = 1; // Count error interrupts disabled
    DFLTCONbits.QEOUT = 1; // Digital filters output enabled for QEn pins
    DFLTCONbits.QECK = 5; // 1:64 clock divide for digital filter for QEn
    //DFLTCONbits.INDOUT = 1; // Digital filter output enabled for Index pin
    //DFLTCONbits.INDCK = 5; // 1:64 clock divide for digital filter for Index
    POSCNT = 0; // Reset position counter
    QEICONbits.QEIM = 6; // X4 mode with position counter reset by Index
    return;
}
 
 
// ------------------ calculate angular speed ------------------------
 
void __attribute__((__interrupt__)) _T2Interrupt (void)
{
 
    IFS0bits.T2IF = 0; // Clear timer 2 interrupt flag
    POSCNTcopy = (int)POSCNT;
    if (POSCNTcopy < 0)
    POSCNTcopy = -POSCNTcopy;
    AngPos[1] = AngPos[0];
    AngPos[0] = POSCNTcopy;
    Veli = AngPos[0] - AngPos[1];
 
    if (Veli >= 0)
    {
        if (Veli >= (HALFMAXSPEED))
            Veli = Veli - MAXSPEED;
    }
    else
    {
        if (Veli < -(HALFMAXSPEED))
           Veli = Veli + MAXSPEED;
    }
 
    Veli *= 2;
 
 
    Velf = (double)Veli;
    Velf = MAXRPS*Velf/MAXSPEEDRAW;
 
    if (!QEICONbits.UPDN)  {  // adjust direction
        Veli = -Veli;
        Velf = -Velf;
    }
 
 
}

Comments

comments

Comments are closed.