File content as of revision 23:1839085ffdcf:

/******************************************************************************/
// ECE4333
// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
// LAB Partner 2:   Brandon Kingman - ID: 3470444
// Project:         Autonomous Robot Design
// Instructor:      Prof. Chris Diduch
/******************************************************************************/
// filename: PiControlThread.cpp
// file content description:
//      * Function to Create the PiControl Thread
//      * PiControl Thread
//      * PiControl ISR
//      * Variables related to the functionality of the thread
/******************************************************************************/

#include "mbed.h"
#include "ui.h"
#include "Drivers/motor_driver_r.h"
#include "Drivers/motor_driver_l.h"
#include "Drivers/DE0_driver.h"
#include "PiControlThread.h"
#include "Drivers/PiController.h"
#include "ui.h"
#include "CameraThread.h"



// setpoints
// speed
int setpointR = 0;
int setpointL = 0;

dp_t dpArray;

int iEnd;

bool memoryFull = false;
bool patrolStart = true;
bool reverse = true;
bool do180 = true;
int cnt=0;

//
int velR, velL;

// control signal
int32_t U_right, U_left;

sensors_t sensors;

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

// steering gain
float Ks = 0.15;
// distance gain
float Kd = 10;

// overall robot required speed
int Setpoint;
Mutex mutexSetpoint;

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

/*******************************************************************************
* @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_R_init();        // initialize motorDriver
    motorDriver_L_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.010);   // 10ms sampling period -> 100Hz freq

}

/*******************************************************************************
* @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)
{
    dpArray.i = 0;
    dpArray.size = ARRAY_SIZE;

    while (true) {

        osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.

        // Get incremental position and time from QEI
        DE0_read(&sensors);

        // might not be needed
        sensors.dp_right = SaturateValue(sensors.dp_right, 112);
        sensors.dp_left = SaturateValue(sensors.dp_left, 112);

        // Maximum angular velocity @ dPostition = 112 is vel = 703 rad/sec
        //velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ;

        // Maximum angular velocity @ dPostition = 112 is vel = 703 rad/sec
        //velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ;

        // if manual control is ON use Differential drive
        if(MC) {
            /******************Manual Setpoint and Steering********************/
            mutexSetpoint.lock();
            setpointR = Setpoint + (Ks*SteeringError);
            setpointL = Setpoint - (Ks*SteeringError);
            mutexSetpoint.unlock();
            /******************Differential End********************************/

            // Make sure our limit is not exceeded
            setpointR = SaturateValue(setpointR, 112);
            setpointL = SaturateValue(setpointL, 112);

            /**********************Record Start********************************/
            // Record dp if there is room in memory
            if( (dpArray.i < dpArray.size) ) {

                // only record data when record button is pressed
                if(startRecording) {
                    dpArray.dpR[dpArray.i] = setpointR;
                    dpArray.dpL[dpArray.i] = setpointL;
                    dpArray.i++;
                    patrolStart = true;
                }

            } else {
                // memory is full
                memoryFull = true;
            }
            /************************Record End********************************/
            
            /*********************Playback Start*******************************/
        } else { // if manual control is not ON then Playback data

            // Check if its the first time to enter the patrol mode
            if(patrolStart) {
                // Store end index value
                iEnd = dpArray.i;
                // Reset playback index
                dpArray.i = 0;
                patrolStart = false;
            }

            // if do the 180 degree is true then do a 180 rotation
            if( do180 == true ) {
                // distance = 10ms * cnt * speed
                if ( cnt < 300 ) {
                    setpointR = -10;
                    setpointL = 10;
                    cnt++;
                }
                // exit the 180 code when done
                else {
                    do180 = false;
                    cnt = 0;
                }
            }
            else if ( dpArray.i < iEnd ) {
                
                // Data Playback normal direction
                if(!reverse) {
                    setpointR = dpArray.dpR[dpArray.i];
                    setpointL = dpArray.dpL[dpArray.i];
                }
                // Data Playback in reverse direction
                else {
                // Wheel dps are swaped to mantain correct direction playback
                    setpointR = dpArray.dpL[dpArray.i];
                    setpointL = dpArray.dpR[dpArray.i];
                }
                dpArray.i++;

            } else {
                // We reached the end of our playback we need to toggle reverse
                reverse = !reverse;
                // start from the beginning by reseting the index
                dpArray.i = 0;
                // Do a 180 degree turn
                do180 = true;
            }


            /***********************Playback End*******************************/
        }

        U_right = PiControllerR(setpointR,sensors.dp_right);
        U_left  = PiControllerL(setpointL,sensors.dp_left);

        // Set speed and direction for right motor
        if (U_right >= 0) {
            motorDriver_R_forward(U_right);
        } else if (U_right < 0) {
            motorDriver_R_reverse(U_right);
        }

        // Set speed and direction for left motor
        if (U_left >= 0) {
            motorDriver_L_forward(U_left);
        } else if (U_left < 0) {
            motorDriver_L_reverse(U_left);
        }

    }

}

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