Hormone for optimizing COT
Dependents: TurtleBot_v01 TurtleBot_with_AHS
hormone.cpp
- Committer:
- worasuchad
- Date:
- 2018-08-13
- Revision:
- 0:df6e371d8665
- Child:
- 1:43249c4dae57
File content as of revision 0:df6e371d8665:
#include "hormone.h" #include "mbed.h" /*------------------------< Function comment >------------------------*/ /* NAME : constructor */ /* PARAMETERS : - */ /* RETURN VALUE : - */ /* DESCRIPTION : - */ /*--------------------------------------------------------------------*/ hormone::hormone() { upDegree = 45.00; downDegree = 95.00; cgUp = 45.00; cgDown = 95; cgUpPrev = 0.00; cgDownPrev = 0.00; upPreProc = 0.0; downPreProc = 0.0; } /*------------------------< Function comment >------------------------*/ /* NAME : hormone concentration */ /* PARAMETERS : - */ /* RETURN VALUE : - */ /* DESCRIPTION : - */ /*--------------------------------------------------------------------*/ float hormone::upHG(const float G2,const float G4 ) { upPreProc = (G2 + G4) / 2; // hormone gland // cgUp = tanh( (0.7f * upPreProc) + (0.3f * cgUpPrev) ); cgUpPrev = cgUp; return 75.00f * cgUp; } /*------------------------< Function comment >------------------------*/ /* NAME : hormone concentration for servo down */ /* PARAMETERS : - */ /* RETURN VALUE : - */ /* DESCRIPTION : - */ /*--------------------------------------------------------------------*/ float hormone::downHG(const float G2,const float G3 ) { downPreProc = (G2 + G3) / 2; // hormone gland // cgDown = tanh( (0.3f * downPreProc) + (0.5f * cgDownPrev) ); cgDownPrev = cgDown; return (-25.00f * cgDown) + 107.5f; } /*------------------------< 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; } /*--------------------------------------------------------------------*/