File content as of revision 0:a355e511bc5d:

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

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

uint16_t ID, dPosition, dTime;
int e, u, xState;

float Ki, Kp;

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
    
    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); 
}


/*******************************************************************************
*               ******** Periodic Timer Interrupt Thread ********
*******************************************************************************/
void PiControlThread(void const *argument)
{  
    while (true) 
    {
        
        osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
        led3= !led3; // Alive status - led3 toggles each time PieriodicZInterruptsThread is signaled.
        
        // get incremental position and time from QEI
        DE0_read(&ID, &dPosition, &dTime);
        
        setpoint_mutex.lock();
        e = setpoint-dPosition;  // e is the velocity error
        setpoint_mutex.unlock();
        
        xState = xState + e; // x is the Euler approximation to the integral of e.
        u = Kp*e + Ki*xState; // u is the control signal
              
        
        if (u >= 0)
        {
            motorDriver_forward(u);
        }
        else if (u < 0)
        {
            motorDriver_reverse(u);
        }
        else
        {
            pc.printf("\r\nerror!!!");
        }
    }
}

/*******************************************************************************
* 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); 
}