TI C2000: Control – enhanced Quadrature Encoder Pulse Module (eQEP)

typedef struct {
	enum eQep_e {
	    EQEP1,
	    EQEP2,
	    EQEP3
	} eq;
	uint32_t cpr;
	int pol;
	float res;
	float init_rad_coord;
} eqep_t;

static eqep_t eqep1;

#define TWOPI 6.283185307179586476925286766559

void init_EQEP1(uint32_t cpr, int pol, float init_rad_coord) 
{
    if ((cpr == 0U) || (cpr > UINT32_MAX / 4U)) {
        return;
    }
    
    if ((pol != 1) && (pol != -1)) {
        return;
    }
    
    eqep1.eq = EQEP1;
    eqep1.cpr = 4*cpr;
    eqep1.init_rad_coord = init_rad_coord;
    eqep1.res = TWOPI/(float)eqep1.cpr; // eqep1.cpr != 0
    eqep1.pol = pol;
    
    // Setup eQEP1 pins for input
    EALLOW;
    GpioCtrlRegs.GPAPUD.bit.GPIO20   = 1; // Disable pull-up on GPIO20 (EQEP1A) 
    GpioCtrlRegs.GPAPUD.bit.GPIO21   = 1; // Disable pull-up on GPIO21 (EQEP1B) 
    GpioCtrlRegs.GPAQSEL2.bit.GPIO20 = 2; // Qual every 6 samples
    GpioCtrlRegs.GPAQSEL2.bit.GPIO21 = 2; // Qual every 6 samples
    GpioCtrlRegs.GPAMUX2.bit.GPIO20  = 1; // Configure GPIO20 as EQEP1A
    GpioCtrlRegs.GPAMUX2.bit.GPIO21  = 1; // Configure GPIO21 as EQEP1B
    EDIS;

    EQep1Regs.QEPCTL.bit.QPEN      = 0;   // Make sure qep reset
    EQep1Regs.QDECCTL.bit.QSRC     = 0;   // Quadrature count mode
    EQep1Regs.QPOSCTL.all          = 0x0; // Disable eQep Position Compare
    EQep1Regs.QCAPCTL.all          = 0x0; // Disable eQep Capture
    EQep1Regs.QEINT.all            = 0x0; // Disable all eQep interrupts
    EQep1Regs.QPOSMAX              = 0xFFFFFFFF;
    EQep1Regs.QEPCTL.bit.FREE_SOFT = 2; 
    EQep1Regs.QEPCTL.bit.QPEN      = 1;   // Enable EQep
    EQep1Regs.QPOSCNT              = 0;
}

float EQEP_read(eqep_t *eqep)
{
    uint32_t raw_unsigned = 0;
    int32_t raw_signed;
    
    switch (eqep->eq) {
        case EQEP1:
            raw_unsigned = EQep1Regs.QPOSCNT;
            break;
        case EQEP2:
            raw_unsigned = EQep2Regs.QPOSCNT;
            break;
        case EQEP3:
            raw_unsigned = EQep3Regs.QPOSCNT;
            break;
        default:
            return 0.0F;    
    }
    
    raw_signed = (int32_t)raw_unsigned;
    
    return ((eqep->pol)*(eqep->res)*raw_signed + eqep->init_rad_coord);
}

float read_Enc1(void) {
    return(EQEP_read(&eqep1));
}

float enc_reading;

void main(void)
{
    // ...
    
    // 100: total slits of motor's encoder in one revolution
    // 30: gear ratio, 30 * 100 = 3000 
    init_EQEP1(3000, 1, 0);
    
    while(1) {
        enc_reading = read_Enc1();
        // Do something else  
    }
}

typedef struct {
	enum eQep_e {
	    EQEP1,
	    EQEP2,
	    EQEP3
	} eq;
	uint32_t cpr;
	int pol;
	float res;
	float init_rad_coord;
} eqep_t;

static eqep_t eqep1;

#define TWOPI 6.283185307179586476925286766559

void init_EQEP1(uint32_t cpr, int pol, float init_rad_coord) 
{
    if ((cpr == 0U) || (cpr > UINT32_MAX / 4U)) {
        return;
    }
    
    if ((pol != 1) && (pol != -1)) {
        return;
    }
    
    eqep1.eq = EQEP1;
    eqep1.cpr = 4*cpr;
    eqep1.init_rad_coord = init_rad_coord;
    eqep1.res = TWOPI/(float)eqep1.cpr; // eqep1.cpr != 0
    eqep1.pol = pol;
    
    // Setup eQEP1 pins for input
    EALLOW;
    GpioCtrlRegs.GPAPUD.bit.GPIO20   = 1; // Disable pull-up on GPIO20 (EQEP1A) 
    GpioCtrlRegs.GPAPUD.bit.GPIO21   = 1; // Disable pull-up on GPIO21 (EQEP1B) 
    GpioCtrlRegs.GPAQSEL2.bit.GPIO20 = 2; // Qual every 6 samples
    GpioCtrlRegs.GPAQSEL2.bit.GPIO21 = 2; // Qual every 6 samples
    GpioCtrlRegs.GPAMUX2.bit.GPIO20  = 1; // Configure GPIO20 as EQEP1A
    GpioCtrlRegs.GPAMUX2.bit.GPIO21  = 1; // Configure GPIO21 as EQEP1B
    EDIS;

    EQep1Regs.QEPCTL.bit.QPEN      = 0;   // Make sure qep reset
    EQep1Regs.QDECCTL.bit.QSRC     = 0;   // Quadrature count mode
    EQep1Regs.QPOSCTL.all          = 0x0; // Disable eQep Position Compare
    EQep1Regs.QCAPCTL.all          = 0x0; // Disable eQep Capture
    EQep1Regs.QEINT.all            = 0x0; // Disable all eQep interrupts
    EQep1Regs.QPOSMAX              = 0xFFFFFFFF;
    EQep1Regs.QEPCTL.bit.FREE_SOFT = 2; 
    EQep1Regs.QEPCTL.bit.QPEN      = 1;   // Enable EQep
    EQep1Regs.QPOSCNT              = 0;
}

float EQEP_read(eqep_t *eqep)
{
    uint32_t raw_unsigned = 0;
    int32_t raw_signed;
    
    switch (eqep->eq) {
        case EQEP1:
            raw_unsigned = EQep1Regs.QPOSCNT;
            break;
        case EQEP2:
            raw_unsigned = EQep2Regs.QPOSCNT;
            break;
        case EQEP3:
            raw_unsigned = EQep3Regs.QPOSCNT;
            break;
        default:
            return 0.0F;    
    }
    
    raw_signed = (int32_t)raw_unsigned;
    
    return ((eqep->pol)*(eqep->res)*raw_signed + eqep->init_rad_coord);
}

float read_Enc1(void) {
    return(EQEP_read(&eqep1));
}

float enc_reading;

void main(void)
{
    // ...
    
    // 100: total slits of motor's encoder in one revolution
    // 30: gear ratio, 30 * 100 = 3000 
    init_EQEP1(3000, 1, 0);
    
    while(1) {
        enc_reading = read_Enc1();
        // Do something else  
    }
}