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: PiController.cpp
// file content description:
//      * PiController initialization function
//      * Picontroller function
//      * Value Saturation functions
/******************************************************************************/

#include "mbed.h"
#include "PiController.h"
#include "ui.h"

// global speed variable;
float Ki;
float Kp;
int32_t scale;

/******************************************************************************/
int SaturatingSubtract(int x, int y)
{
    int z;
    z = x - y; // 32-bit overflow detection and saturating arithmetic
    if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF;
    else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000;
    return z;
}

/******************************************************************************/
int SaturatingAdd(int x, int y)
{
    int z;
    z = x + y; // 32-bit overflow detection and saturating arithmetic
    if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF;
    else if((x < 0) && (y < 0) && (z > 0)) z = 0x80000000;
    return z;
}

/******************************************************************************/
int SaturateValue(int x, int Limit)
{
    if(x > Limit) return(Limit); // Impose maximum limit on x
    else if(x < -Limit) return(-Limit);
    else return(x);
}


/*****************************************************************************/
void PiController_init(float Kp_given, float Ki_given)
{
    Kp = Kp_given;
    Ki = Ki_given;
    
    // initialization
    scale = 40;
    //xState = 0;  
        
}


/*******************************************************************************
* @brief    PI Controller function
* @param    setp is the setpoint we would like the system to get to
* @param    dP is the current speed of the system 
* @return   u is the control signal out of the controller to make the system 
*           reach the desired setpoint
*******************************************************************************/
uint32_t PiControllerR(int setp, int dP)
{
    static int32_t e, u; 
    static int32_t xState = 0;
    int32_t xTemp;
    int32_t uProportional;
    int32_t uIntegral;
    int32_t uS;
    
    setpointR_mutex.lock();
    e = SaturatingSubtract(setp, dP);  // e is the velocity error
    setpointR_mutex.unlock();

    xTemp = SaturatingAdd(xState, e);

    // the maximum value that 'u' can get to is 20
    // the maximum value that dPosition can get to 560
    // scaling factor is 560/20 = 28
    // scaling factor used is 40
    
    uProportional = (float)(Kp*e/scale);
    uIntegral = (float)(Ki*xState/scale);

    uS = SaturatingAdd(uProportional, uIntegral);

    u = SaturateValue(uS, U_LIMIT);
    if(u==uS) xState=xTemp; // if limit has not been reached then update xState
    
    return u;
    
}


/*******************************************************************************
* @brief    PI Controller function
* @param    setp is the setpoint we would like the system to get to
* @param    dP is the current speed of the system 
* @return   u is the control signal out of the controller to make the system 
*           reach the desired setpoint
*******************************************************************************/
uint32_t PiControllerL(int setp, int dP)
{
    static int32_t e, u;
    static int32_t xState = 0;
    int32_t xTemp;
    int32_t uProportional;
    int32_t uIntegral;
    int32_t uS;
    
    setpointL_mutex.lock();
    e = SaturatingSubtract(setp, dP);  // e is the velocity error
    setpointL_mutex.unlock();

    xTemp = SaturatingAdd(xState, e);

    // the maximum value that 'u' can get to is 20
    // the maximum value that dPosition can get to 560
    // scaling factor is 560/20 = 28
    // scaling factor used is 40
    
    uProportional = (float)(Kp*e/scale);
    uIntegral = (float)(Ki*xState/scale);

    uS = SaturatingAdd(uProportional, uIntegral);

    u = SaturateValue(uS, U_LIMIT);
    if(u==uS) xState=xTemp; // if limit has not been reached then update xState
    
    return u;
    
}