eMotor Phones Home

Moving on to our main discussion, the PID algorithm, which is implemented as described on page 3 in my earlier article Digital PID Controllers. I put the algorithm in timer 1 ISR. For the eMotor with maximum speed of only 600 RPM (or 10 rev.per sec), the servo rate does not have to be very fast. Indeed, the speed calculation is done in timer 2 at sampling period 50 milliseconds. So I choose PID cycle at 10 miliseconds. The list below shows the PID algorithm used.

// ----------- 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;
    }
 
 
}

For more information about timer ISR, read our ECS Lab 1: Timer Basics

The code appears more complicated than in my PID article. First, I add more fancy stuff such as peformance indicator LEDs. Second, the algorithm must choose the source of speed command, either from VR or an internal variable adjusted from a command line (sent via UART interface). And last, the control variable must be converted to a pair of PWM and DIR signals, the scheme that eMotor accepts.

To clarify some variables used, SysFlag is a structure that keeps system status. For example, SysFlag.VelLoop equals 1 (TRUE) when the velocity feedback loop is closed. From the code above, it means _T1Interrput is idle when SysFlag.VelLoop is 0(FALSE). The open-loop response is commaned from the main loop, which contains quite similar mechanisms except there is no PID and feedback.

When implementing a periodic timer routine, one must beware of the timing constraint; i.e., any chunk of code put in there must finish excution before the timer period expires. Experienced programmers may advice against calling any function from an ISR. This is not an issue for our pretty slow timer routine, however.

Some points worth mentioning about timing and variable types/values. To simplify the algorithm a bit, I use floating-point variables (of type double) in the PID computation. the dsPIC does not have a hardware FP unit so it does that by firmware, which means more machine instructions and longer loop time. For a real application with more stringent timing requirement, you might choose integer arithmetic. In such case, the range of variable type must be taken in to consideration, say, a 16-bit integer range is -32768 to +32767. Also pay special attention in an equation that involves variables of different types. Use type casting where necessary. Disregarding these issues could cause some bug difficult to trace.

One advantage of experimenting with the eMotor is when we do something stupid (as I too often do), the eMotor LEDs are either flashing like crazy (closed-loop unstable) or the CPU halts (program bugs, too small sampling period). No physical harm could happen to human or the system, unlike a real motor mounted on a robot joint. When a student understands the effect of adjusting the PID gains, he/she could move on confidently to a real motion control application.

Comments

comments

Comments are closed.