CCS/EK-TM4C123GXL: Velocity Control

Tool/software: Code Composer Studio

Hi all,

I am trying to control velocity of DC motor using PID, but i am have trouble to stabilize it any desired velocity, and also the derivative terms stays zero all the time.

/*   DC Motor Velocity    */

#include "Config.h"

#define PERIOD_PWM 1250 // 2ms time period is equal to 1250 ticks

#define GET_MAG(X)  (X > 0 ? X : -X)

#ifndef DEGPS_RPM
#define DEGPS_RPM   0.16667
#endif

float encdrRdShldr, rotDirShldr, dpsShldr, rpmShldr, errVelShldr, intgErrShldr, dErrShldr, pidVelShldr, prevErrVelShldr;
float desVelShldr, kpVelShldr, kiVelShldr, kdVelShldr, pwm,D;
float scaledOutVelShldr;
float accErrVelShldr;
int a;
int main ( void )
{
   SysCtlClockSet ( SYSCTL_SYSDIV_2|SYSCTL_USE_PLL|SYSCTL_OSC_MAIN|SYSCTL_XTAL_16MHZ ) ; //  400/2=200MHz (2 is default divider), 200/5=40MHz (System Clock become 40MHz),

   tmr0IntrptConfig(); // Call timer 0 interrupt Configuration
   encdrShldrConfig(); // Call Shoulder Motor encoder Configuration
   pwmShldrConfig();   // Call PWM Shoulder Configuration
   initlzVarConfig();
   kpVelShldr = 1.5; kiVelShldr = 1; kdVelShldr = 2;
   /*     Direction PINS   */
   SysCtlPeripheralEnable ( SYSCTL_PERIPH_GPIOB ) ;
   GPIOPinTypeGPIOOutput ( GPIO_PORTB_BASE, ( GPIO_PIN_0 | GPIO_PIN_1|GPIO_PIN_2 )  ) ;
mag = 99;
a = 0;
}

void Timer0IntHandler(void)
{
    encdrRdShldr = QEIVelocityGet(QEI0_BASE);
    rotDirShldr = QEIDirectionGet(QEI0_BASE);

    if(rotDirShldr == 1)        //CCW
    {
        dpsShldr = -1*(encdrRdShldr * 0.1124);
        rpmShldr = dpsShldr * DEGPS_RPM;
    }
    else if(rotDirShldr  == -1) //CW
    {
        dpsShldr = (encdrRdShldr * 0.1124);
        rpmShldr = dpsShldr * DEGPS_RPM;
    }

    if(desVelShldr > 60)
    {
        desVelShldr = 60;
    }   else if(desVelShldr < -60)
    {   desVelShldr = -60;
    }
    errVelShldr  = desVelShldr - rpmShldr;
    intgErrShldr = (accErrVelShldr) * 0.0001;
    dErrShldr    = (errVelShldr - prevErrVelShldr) * 10000;
    pwm = desVelShldr * 1.35; //  pwm according to desired velociety
    pidVelShldr  = (kpVelShldr * errVelShldr + kiVelShldr * intgErrShldr + kdVelShldr * dErrShldr)+pwm;
    D= kdVelShldr * dErrShldr;


    accErrVelShldr += errVelShldr;

    if(pidVelShldr >= 0)
    {
        if(pidVelShldr >= 254)  pidVelShldr = 254;
        if(pidVelShldr <= 1)    pidVelShldr = 1;
    }
    else if(pidVelShldr < 0)
    {
        if(pidVelShldr <= -254) pidVelShldr = -254;
        if(pidVelShldr >= -1)   pidVelShldr = -1;
    }

    if(pidVelShldr >= 0)
    {
        //Set direction bits to rotate motor CW
        GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_0, GPIO_PIN_0);
        GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_1, 0x00);
    }
    else
    {
        //Set direction bits to rotate motor CCW
        GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_0, 0x00);
        GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_1, GPIO_PIN_1);
    }

    scaledOutVelShldr = GET_MAG(pidVelShldr) * 5.0;
    if(scaledOutVelShldr > 1250)
        {scaledOutVelShldr = 1250;}
    PWMPulseWidthSet ( PWM1_BASE, PWM_OUT_7, scaledOutVelShldr);

    a++;
    if(a == 1){
        GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_2, GPIO_PIN_2);
    }if(a == 2){
    GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_2, 0x00);
    a = 0;}
    prevErrVelShldr = errVelShldr;
    TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);// Clear the timer interrupt

}

and this is code for Config.c source file

#include "Config.h"

float mag;

void initlzVarConfig (void)
{
mag = 0.0;
}

void encdrShldrConfig (void)
{
SysCtlPeripheralEnable ( SYSCTL_PERIPH_GPIOD ) ;
//Unlock GPIOD7
HWREG (GPIO_PORTD_BASE + GPIO_O_LOCK) = GPIO_LOCK_KEY;
HWREG (GPIO_PORTD_BASE + GPIO_O_CR) |= 0x80;
HWREG (GPIO_PORTD_BASE + GPIO_O_AFSEL) &= ~0x80;
HWREG (GPIO_PORTD_BASE + GPIO_O_DEN) |= 0x80;
HWREG (GPIO_PORTD_BASE + GPIO_O_LOCK) = 0;

SysCtlPeripheralEnable ( SYSCTL_PERIPH_QEI0 ) ; // Enable QEI Peripherals
//Set Pins to be PHA0 and PHB0
GPIOPinConfigure(GPIO_PD6_PHA0); //GPIOPinConfigure ( 0x00031806 ) ; //0x00031806 =>GPIO_PD6_PHA0
GPIOPinConfigure(GPIO_PD7_PHB0); //GPIOPinConfigure ( 0x00031C06 ) ; // 0x00031C06 => GPIO_PD7_PHB0
GPIOPinTypeQEI(GPIO_PORTD_BASE, (GPIO_PIN_6 | GPIO_PIN_7)) ; //Set GPIO pins for QEI

// Configure quadrature encoder, use an arbitrary top limit of 2000 and enable QEI
QEIConfigure (QEI0_BASE, (QEI_CONFIG_CAPTURE_A | QEI_CONFIG_RESET_IDX | QEI_CONFIG_QUADRATURE | QEI_CONFIG_NO_SWAP) ,640000) ;
QEIEnable(QEI0_BASE);

//Configure and enable velocity
QEIVelocityConfigure(QEI0_BASE, QEI_VELDIV_1, SysCtlClockGet()) ; // Divide by clock speed to get counts/sec
QEIVelocityEnable(QEI0_BASE);
}

void tmr0IntrptConfig (void)
{
SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0) ;
TimerConfigure(TIMER0_BASE, TIMER_CFG_A_PERIODIC) ;
uint32_t InterruptPeriod = SysCtlClockGet() * 0.0001;
TimerLoadSet(TIMER0_BASE, TIMER_A, InterruptPeriod - 1) ;
IntEnable(INT_TIMER0A);
TimerIntEnable(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
IntMasterEnable();
TimerEnable(TIMER0_BASE, TIMER_A) ;
}

void pwmShldrConfig (void)
{
// Enable the peripherals used by this program.
SysCtlPeripheralEnable ( SYSCTL_PERIPH_PWM1 ) ;
while(!SysCtlPeripheralReady(SYSCTL_PERIPH_PWM1));
SysCtlPeripheralEnable ( SYSCTL_PERIPH_GPIOF ) ;
GPIOPinTypePWM ( GPIO_PORTF_BASE, GPIO_PIN_3 ) ; // Configure PF3 Pins as PWM
GPIOPinConfigure ( GPIO_PF3_M1PWM7 ) ;
SysCtlPWMClockSet ( SYSCTL_PWMDIV_64 ) ; // 40/64=0.625 MHz, 625KHz
PWMDeadBandDisable ( PWM1_BASE,PWM_GEN_3 ) ;
PWMGenConfigure ( PWM1_BASE, PWM_GEN_3, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_NO_SYNC ) ; //Configure PWM Options
uint32_t PWMPeriod=1250; //PWM_GEN_2 Covers M1PWM4 and M1PWM5
PWMGenPeriodSet ( PWM1_BASE, PWM_GEN_3, PWMPeriod ) ; //PWM_GEN_3 Covers M1PWM6 and M1PWM7
PWMOutputState ( PWM1_BASE, PWM_OUT_7_BIT, true ) ; //Turn on the Output pins
PWMGenEnable ( PWM1_BASE, PWM_GEN_3 ) ; //Enable the PWM generator
}