File content as of revision 16:58ec2b891a25:

/******************************************************************************/
// 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


// 
int velR, velL;

// control signal
int32_t U_right, U_left;

sensors_t sensors;

int time_passed = 0;

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

// steering gain
float Ks = 0.15;

// distance gain
float Kd = 10;

// overall robot required speed
int Setpoint;

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_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.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(&sensors);
        
        // might not be needed
        sensors.dp_right = SaturateValue(sensors.dp_right, 560);
        sensors.dp_left = SaturateValue(sensors.dp_left, 560);
        
        // Maximum velocity at dPostition = 560 is vel = 703
        velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ;
        
        // Maximum velocity at dPostition = 560 is vel = 703
        velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ;
        
        /*********************Differential Start*******************************/
        // Inputs are Speed Setpoint and Steering Setpoint
        // The Inputs for the Steering are specified in the CameraThread
        // and set at the center.
        // The distance between the object and the image should be set to 1 meter   
        // If distance decrease then speed should stop.
        // If distance increase then speed should increase.
        
        // If object is moving away from the the robot increase robot speed
        cameraData_mutex.lock();
        if(DistanceError > 0)
        {
            // Proportional controller
            Setpoint = (Kd*DistanceError);
            Setpoint = SaturateValue(Setpoint,560);
        }
        // If object is at the set distance limit or less then do not move.
        else if(DistanceError <= 0)
        {
            Setpoint = 0;
        }
        
        // Decoupled drive  
        setpointR = Setpoint + (Ks*SteeringError);
        setpointL = Setpoint - (Ks*SteeringError);
        cameraData_mutex.unlock();
        
        // Make sure our limit is not exceeded
        setpointR = SaturateValue(setpointR, 560);
        setpointL = SaturateValue(setpointL, 560);
       
        /*********************Differential 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 of 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 50ms
* @param    none
* @return   none
*******************************************************************************/
void PeriodicInterruptISR(void)
{
    // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
    osSignalSet(PiControlId,0x1); 
}