Hormone for optimizing COT
Dependents: TurtleBot_v01 TurtleBot_with_AHS
Diff: hormone.cpp
- Revision:
- 0:df6e371d8665
- Child:
- 1:43249c4dae57
diff -r 000000000000 -r df6e371d8665 hormone.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hormone.cpp Mon Aug 13 11:33:22 2018 +0000 @@ -0,0 +1,99 @@ +#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; +} +/*--------------------------------------------------------------------*/ \ No newline at end of file