File content as of revision 7:73fd05fe556a:

#include "mbed.h"
#include "ui.h"
#include "Drivers/motor_driver.h"
#include "Drivers/DE0_driver.h"
#include "PiControlThread.h"
#include "Drivers/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);

/*******************************************************************************
* @brief    function that creates a thread for the PI controller. It initializes 
*           the PI controller's gains and initializes the DC Motor. It also 
*           initializes the PIControllerThread runs at 50ms period
* @param    none
* @return   none
*******************************************************************************/
void PiControlThreadInit()
{
    DE0_init();                  // initialize FPGA
    motorDriver_init();          // initialize motorDriver
    //                Kp,Ki
    PiController_init(1,0.4); // 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
     
}


/*******************************************************************************
* @brief    This is the PI controller thread. It reads several values from the 
*           FPGA such as speed, time and other sensors data
* @param    none
* @return   none
*******************************************************************************/
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);
        }
           
    }
    
}

/*******************************************************************************
* @brief    The ISR below signals the PIControllerThread. it is setup to run 
*           every 50ms
* @param    none
* @return   none
*******************************************************************************/
void PeriodicInterruptISR(void)
{
    // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
    osSignalSet(PiControlId,0x1); 
}