File content as of revision 4:417e475239c7:

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

extern int setpoint;

uint16_t ID, dTime;
int dPosition;
int vel;
int32_t U;

void PiControlThread(void const *);
void PeriodicInterruptISR(void);

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
    PiController_init(2.2,0.01); // initialize the PI Controller gains and initialize variables
    
    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
     
}


/*******************************************************************************
*               ******** Periodic Timer Interrupt Thread ********
*******************************************************************************/
void PiControlThread(void const *argument)
{
    
    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) ;
        
        U = PiController(setpoint,dPosition);
        
        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); 
}