File content as of revision 5:b29220d29022:

#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;
extern bool start_counting;
extern Serial bluetooth;

uint16_t ID, dTime;
int dPosition;
int vel;
int32_t U;
int counter =0 ;

int dPosition_values[1000];

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.001);   // 50ms sampling rate

}


/*******************************************************************************
*               ******** Periodic Timer Interrupt Thread ********
*******************************************************************************/
void PiControlThread(void const *argument)
{

    // time constant calc
    setpoint = 140;

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

        // time constnant calc.
        U = setpoint/28;

        if (U >= 0) {
            motorDriver_forward(U);
        } else if (U < 0) {
            motorDriver_reverse(U);
        }


        // start our calculations
        if( start_counting == true ) {
            float time_constant = 0;
            dPosition_values[counter] = dPosition;
            counter++;
            
            // if we reach our setpoint
            if (dPosition > 8 ) {
                // stop counting time
                start_counting = false;
                // find the value where dPosition = 316
                for(int i=0; i<1000; i++) {
                    if( dPosition_values[i] >= 7 ) {
                        time_constant = counter;
                        //time_constant = time_constant;
                        bluetooth.printf("\ntime constant: %f", time_constant);
                    }
                }
            }
        }





    }

}

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