Hormone for optimizing COT
Dependents: TurtleBot_v01 TurtleBot_with_AHS
hormone.cpp
- Committer:
- worasuchad
- Date:
- 2019-11-10
- Revision:
- 3:91e508c05dca
- Parent:
- 2:6da425fac1f7
- Child:
- 4:0dfa16b90d92
File content as of revision 3:91e508c05dca:
#include "hormone.h" #include "mbed.h" #define HORMONE 0 // do you want to use hormone or not /*------------------------< Function comment >------------------------*/ /* NAME : constructor */ /* PARAMETERS : - */ /* RETURN VALUE : - */ /* DESCRIPTION : - */ /*--------------------------------------------------------------------*/ hormone::hormone() { // default config // upDegree = 45.00; downDegree = 90.00; cgUp = 45.00; cgDown = 90.00; cgUpPrev = 0.6; cgDownPrev = 0.7; /* // flat config // upDegree = 45.00; downDegree = 95.00; cgUp = 45.00; cgDown = 95; cgUpPrev = 0.6; cgDownPrev = 0.5; */ /* // small config // upDegree = 60.00; downDegree = 100.00; cgUp = 60.00; cgDown = 100; cgUpPrev = 0.8; cgDownPrev = 0.3; */ /* // big config // upDegree = 75.00; downDegree = 90.00; cgUp = 75.00; cgDown = 90; cgUpPrev = 1.0; cgDownPrev = 0.7; */ 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 return 45.00f; #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 return 95.00f; #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; } /*--------------------------------------------------------------------*/