File content as of revision 3:4def4ca68910:

#include "mbed.h"
#include "ui.h"
#include "Drivers/motor_driver.h"
#include "Drivers/DE0_driver.h"
#include "PiControlThread.h"

// global speed variable;
extern int setpoint;
extern Serial pc;
extern Mutex setpoint_mutex;

uint16_t ID, dTime;
int dPosition;
int vel;

int32_t e, u, xState;

bool saturationFlag;

float Ki; 
float Kp;

void PiControlThread(void const *);
void PeriodicInterruptISR(void);
int SaturatingSubtract(int , int );
int SaturatingAdd(int , int );
int SaturateValue(int , int );


osThreadId PiControlId;

/******************************************************************************/
//  osPriorityIdle          = -3,          ///< priority: idle (lowest)
//  osPriorityLow           = -2,          ///< priority: low
//  osPriorityBelowNormal   = -1,          ///< priority: below normal
//  osPriorityNormal        =  0,          ///< priority: normal (default)
//  osPriorityAboveNormal   = +1,          ///< priority: above normal
//  osPriorityHigh          = +2,          ///< priority: high
//  osPriorityRealtime      = +3,          ///< priority: realtime (highest)
/******************************************************************************/

// Declare PeriodicInterruptThread as a thread/process
osThreadDef(PiControlThread, osPriorityRealtime, 1024); 

Ticker PeriodicInt;      // Declare a timer interrupt: PeriodicInt

DigitalOut led3(LED3);


void PiControlThreadInit()
{
    DE0_init();             // initialize FPGA
    motorDriver_init();     // initialize motorDriver
    
    PiControlId = osThreadCreate(osThread(PiControlThread), NULL);

    // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval
    // in seconds between interrupts, and start interrupt generation:
    PeriodicInt.attach(&PeriodicInterruptISR, 0.05);   // 50ms sampling rate
    
    Kp = 2.2;
    Ki = 0.01;
    
}


/*******************************************************************************
*               ******** Periodic Timer Interrupt Thread ********
*******************************************************************************/
void PiControlThread(void const *argument)
{
    
    // initialization
    saturationFlag = false;
    int scale = 40;
    xState = 0;  
    
    int32_t xTemp;
    int32_t uProportional;
    int32_t uIntegral;
    int32_t uS;
      
    while (true) 
    {
        osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
        
        // get incremental position and time from QEI
        DE0_read(&ID, &dPosition, &dTime);
        SaturateValue(dPosition, 560);
        
        // maximum velocity at dPostition = 560 is vel = 703
        vel = (float)((6135.92 * dPosition) / dTime) ;
        
        setpoint_mutex.lock();
        e = SaturatingSubtract(setpoint, dPosition);  // e is the velocity error
        setpoint_mutex.unlock();
            
        xTemp = SaturatingAdd(xState, e);
        
        // the maximum value that 'u' can get to is 20
        // the maximum value that dPosition can get to 560
        // scaling factor is 560/20 = 28
        uProportional = (float)(Kp*e/scale);
        uIntegral = (float)(Ki*xState/scale);
        
        uS = SaturatingAdd(uProportional, uIntegral);
        
        u = SaturateValue(uS, U_LIMIT);      
        if(u==uS) xState=xTemp;
        
        if (u >= 0)
        {
            motorDriver_forward(u);
        }
        else if (u < 0)
        {
            motorDriver_reverse(u);
        }   
           
    }
    
}

/*******************************************************************************
* the interrupt below occures every 250ms as setup in the main function during 
* initialization
*               ******** Period Timer Interrupt Handler ********
*******************************************************************************/
void PeriodicInterruptISR(void)
{
    // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
    osSignalSet(PiControlId,0x1); 
}


/*****************************************************************************/
int SaturatingSubtract(int x, int y)
{
    int z;
    z = x - y; // 32-bit overflow detection and saturating arithmetic
    if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF;
    else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000;
    return z;
}

/*****************************************************************************/
int SaturatingAdd(int x, int y)
{
    int z;
    z = x + y; // 32-bit overflow detection and saturating arithmetic
    if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF;
    else if((x < 0) && (y < 0) && (z > 0)) z = 0x80000000;
    return z;
}

/*****************************************************************************/
int SaturateValue(int x, int Limit)
{
    if(x > Limit) return(Limit); // Impose maximum limit on x
    else if(x < -Limit) return(-Limit);
    else return(x);
}