File content as of revision 4:0dfa16b90d92:

#include "hormone.h"
#include "mbed.h"

#define HORMONE 0       // do you want to use hormone (1) or not (0)

// gait configuration when hormone is disable
#define GAITCONFIG 0    // 0-default, 1-flat, 2-gravel, 3-pebble terrain

/*------------------------< Function comment >------------------------*/ 
/* NAME         : constructor                                         */ 
/* PARAMETERS   : -                                                   */ 
/* RETURN VALUE : -                                                   */ 
/* DESCRIPTION  : -                                                   */ 
/*--------------------------------------------------------------------*/ 
hormone::hormone()
{
    #if(GAITCONFIG == 0)
        // default config //
        upDegree = 45.00; downDegree = 90.00; 
        cgUp = 45.00; cgDown = 90.00;
        //cgUpPrev = 0.6; cgDownPrev = 0.7;
    #endif
    
    #if(GAITCONFIG == 1)
        // flat config //
        upDegree = 45.00; downDegree = 95.00; 
        cgUp = 45.00; cgDown = 95;
        //cgUpPrev = 0.6; cgDownPrev = 0.5;
    #endif

    #if(GAITCONFIG == 2)
        // small config //
        upDegree = 60.00; downDegree = 100.00; 
        cgUp = 60.00; cgDown = 100;
        //cgUpPrev = 0.8; cgDownPrev = 0.3; 
    #endif
    
    #if(GAITCONFIG == 3)
        // big config //
        upDegree = 75.00; downDegree = 90.00; 
        cgUp = 75.00; cgDown = 90;
        //cgUpPrev = 1.0; cgDownPrev = 0.7; 
    #endif
    
    cgUpPrev = 0.0; cgDownPrev = 0.0;
    upPreProc = 0.0; downPreProc = 0.0;
}

/*------------------------< Function comment >------------------------*/ 
/* NAME         : hormone concentration                               */ 
/* PARAMETERS   : -                                                   */ 
/* RETURN VALUE : -                                                   */ 
/* DESCRIPTION  : -                                                   */ 
/*--------------------------------------------------------------------*/ 
float hormone::upHG(const float G2,const float G4 )
{    
    #if(HORMONE)
        upPreProc = (G2 + G4) / 2.00f;
        
        //      hormone gland       //
        cgUp = tanh( (0.7f * upPreProc) + (0.3f * cgUpPrev) );
        cgUpPrev = cgUp;
        
        return 75 * cgUp;
    #else
        #if(GAITCONFIG == 0 || GAITCONFIG == 1 )
            return 45.00f;
        #endif
        
        #if(GAITCONFIG == 2)
            return 60.00f;
        #endif
        
        #if(GAITCONFIG == 3)
            return 75.00f;
        #endif
    #endif
}


/*------------------------< Function comment >------------------------*/ 
/* NAME         : hormone concentration for servo down                */ 
/* PARAMETERS   : -                                                   */ 
/* RETURN VALUE : -                                                   */ 
/* DESCRIPTION  : -                                                   */ 
/*--------------------------------------------------------------------*/ 
float hormone::downHG(const float G2,const float G3 )
{    
    #if(HORMONE)
        downPreProc = (G2 + G3) / 2.00f;
        
        //      hormone gland       //
        cgDown = tanh( (0.3f * downPreProc) + (0.5f * cgDownPrev) );
        cgDownPrev = cgDown;
     
        return (-25.00f * cgDown) + 107.50f;
    #else
        #if(GAITCONFIG == 0 || GAITCONFIG == 3 )
            return 90.00f;
        #endif
        
        #if(GAITCONFIG == 1)
            return 95.00f;
        #endif
        
        #if(GAITCONFIG == 2)
            return 100.00f;
        #endif
    #endif
}

/*------------------------< Function comment >------------------------*/ 
/* NAME         : hormone receiver for servo up                       */ 
/* PARAMETERS   : -                                                   */ 
/* RETURN VALUE : -                                                   */ 
/* DESCRIPTION  : -                                                   */ 
/*--------------------------------------------------------------------*/
float hormone::hormoneRecUp(const float upDeg)
{
    upDegree = upDeg;

    if(upDegree < 45.00f )
    {
        upDegree = 45.00f;
    }
    else if(upDegree > 75.00f)
    {
        upDegree = 75.00f;
    }
    
    return upDegree;
}

/*------------------------< Function comment >------------------------*/ 
/* NAME         : hormone receiver for servo down                     */ 
/* PARAMETERS   : -                                                   */ 
/* RETURN VALUE : -                                                   */ 
/* DESCRIPTION  : -                                                   */ 
/*--------------------------------------------------------------------*/
float hormone::hormoneRecDown(const float downDeg)
{
    //downDegree = 90.00f*(1.00f-(0.06f*cgDown)); 
    downDegree = downDeg;

    if(downDegree < 90.00f)
    {
        downDegree = 90.00f;
    }
    else if(downDegree > 100.00f)
    {
        downDegree = 100.00f;
    }
    
    return downDegree;
}
/*--------------------------------------------------------------------*/