Hormone for optimizing COT
Dependents: TurtleBot_v01 TurtleBot_with_AHS
Diff: hormone.cpp
- Revision:
- 3:91e508c05dca
- Parent:
- 2:6da425fac1f7
- Child:
- 4:0dfa16b90d92
diff -r 6da425fac1f7 -r 91e508c05dca hormone.cpp --- a/hormone.cpp Fri Aug 24 10:55:36 2018 +0000 +++ b/hormone.cpp Sun Nov 10 02:44:56 2019 +0000 @@ -1,6 +1,8 @@ #include "hormone.h" #include "mbed.h" +#define HORMONE 0 // do you want to use hormone or not + /*------------------------< Function comment >------------------------*/ /* NAME : constructor */ /* PARAMETERS : - */ @@ -9,19 +11,25 @@ /*--------------------------------------------------------------------*/ 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; @@ -29,7 +37,7 @@ cgUpPrev = 1.0; cgDownPrev = 0.7; */ - //upPreProc = 0.0; downPreProc = 0.0; + upPreProc = 0.0; downPreProc = 0.0; } /*------------------------< Function comment >------------------------*/ @@ -40,18 +48,17 @@ /*--------------------------------------------------------------------*/ float hormone::upHG(const float G2,const float G4 ) { -/* - upPreProc = (G2 + G4) / 2.00f; - - // hormone gland // - cgUp = tanh( (0.7f * upPreProc) + (0.3f * cgUpPrev) ); - //cgUp = tanh( (0.3f * upPreProc) + (0.5f * cgUpPrev) ); - cgUpPrev = cgUp; - - return 75 * cgUp; - //return (50.00f * cgUp) + 30.00f; -*/ - return 60.00f; + #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 } @@ -63,16 +70,17 @@ /*--------------------------------------------------------------------*/ float hormone::downHG(const float G2,const float G3 ) { -/* - downPreProc = (G2 + G3) / 2.00f; - - // hormone gland // - cgDown = tanh( (0.3f * downPreProc) + (0.5f * cgDownPrev) ); - cgDownPrev = cgDown; - - return (-25.00f * cgDown) + 107.50f; -*/ - return 100.00f; + #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 >------------------------*/