Hormone for optimizing COT
Dependents: TurtleBot_v01 TurtleBot_with_AHS
hormone.cpp@3:91e508c05dca, 2019-11-10 (annotated)
- Committer:
- worasuchad
- Date:
- Sun Nov 10 02:44:56 2019 +0000
- Revision:
- 3:91e508c05dca
- Parent:
- 2:6da425fac1f7
- Child:
- 4:0dfa16b90d92
add config
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
worasuchad | 0:df6e371d8665 | 1 | #include "hormone.h" |
worasuchad | 0:df6e371d8665 | 2 | #include "mbed.h" |
worasuchad | 0:df6e371d8665 | 3 | |
worasuchad | 3:91e508c05dca | 4 | #define HORMONE 0 // do you want to use hormone or not |
worasuchad | 3:91e508c05dca | 5 | |
worasuchad | 0:df6e371d8665 | 6 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 7 | /* NAME : constructor */ |
worasuchad | 0:df6e371d8665 | 8 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 9 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 10 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 11 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 12 | hormone::hormone() |
worasuchad | 0:df6e371d8665 | 13 | { |
worasuchad | 3:91e508c05dca | 14 | // default config // |
worasuchad | 3:91e508c05dca | 15 | upDegree = 45.00; downDegree = 90.00; |
worasuchad | 3:91e508c05dca | 16 | cgUp = 45.00; cgDown = 90.00; |
worasuchad | 3:91e508c05dca | 17 | cgUpPrev = 0.6; cgDownPrev = 0.7; |
worasuchad | 3:91e508c05dca | 18 | |
worasuchad | 2:6da425fac1f7 | 19 | /* |
worasuchad | 2:6da425fac1f7 | 20 | // flat config // |
worasuchad | 0:df6e371d8665 | 21 | upDegree = 45.00; downDegree = 95.00; |
worasuchad | 0:df6e371d8665 | 22 | cgUp = 45.00; cgDown = 95; |
worasuchad | 2:6da425fac1f7 | 23 | cgUpPrev = 0.6; |
worasuchad | 2:6da425fac1f7 | 24 | cgDownPrev = 0.5; |
worasuchad | 3:91e508c05dca | 25 | */ |
worasuchad | 3:91e508c05dca | 26 | /* |
worasuchad | 2:6da425fac1f7 | 27 | // small config // |
worasuchad | 2:6da425fac1f7 | 28 | upDegree = 60.00; downDegree = 100.00; |
worasuchad | 2:6da425fac1f7 | 29 | cgUp = 60.00; cgDown = 100; |
worasuchad | 2:6da425fac1f7 | 30 | cgUpPrev = 0.8; |
worasuchad | 2:6da425fac1f7 | 31 | cgDownPrev = 0.3; |
worasuchad | 3:91e508c05dca | 32 | */ |
worasuchad | 2:6da425fac1f7 | 33 | /* |
worasuchad | 2:6da425fac1f7 | 34 | // big config // |
worasuchad | 2:6da425fac1f7 | 35 | upDegree = 75.00; downDegree = 90.00; |
worasuchad | 2:6da425fac1f7 | 36 | cgUp = 75.00; cgDown = 90; |
worasuchad | 2:6da425fac1f7 | 37 | cgUpPrev = 1.0; |
worasuchad | 2:6da425fac1f7 | 38 | cgDownPrev = 0.7; |
worasuchad | 2:6da425fac1f7 | 39 | */ |
worasuchad | 3:91e508c05dca | 40 | upPreProc = 0.0; downPreProc = 0.0; |
worasuchad | 0:df6e371d8665 | 41 | } |
worasuchad | 0:df6e371d8665 | 42 | |
worasuchad | 0:df6e371d8665 | 43 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 44 | /* NAME : hormone concentration */ |
worasuchad | 0:df6e371d8665 | 45 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 46 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 47 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 48 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 49 | float hormone::upHG(const float G2,const float G4 ) |
worasuchad | 0:df6e371d8665 | 50 | { |
worasuchad | 3:91e508c05dca | 51 | #if(HORMONE) |
worasuchad | 3:91e508c05dca | 52 | upPreProc = (G2 + G4) / 2.00f; |
worasuchad | 3:91e508c05dca | 53 | |
worasuchad | 3:91e508c05dca | 54 | // hormone gland // |
worasuchad | 3:91e508c05dca | 55 | cgUp = tanh( (0.7f * upPreProc) + (0.3f * cgUpPrev) ); |
worasuchad | 3:91e508c05dca | 56 | cgUpPrev = cgUp; |
worasuchad | 3:91e508c05dca | 57 | |
worasuchad | 3:91e508c05dca | 58 | return 75 * cgUp; |
worasuchad | 3:91e508c05dca | 59 | #else |
worasuchad | 3:91e508c05dca | 60 | return 45.00f; |
worasuchad | 3:91e508c05dca | 61 | #endif |
worasuchad | 0:df6e371d8665 | 62 | } |
worasuchad | 0:df6e371d8665 | 63 | |
worasuchad | 0:df6e371d8665 | 64 | |
worasuchad | 0:df6e371d8665 | 65 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 66 | /* NAME : hormone concentration for servo down */ |
worasuchad | 0:df6e371d8665 | 67 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 68 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 69 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 70 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 71 | float hormone::downHG(const float G2,const float G3 ) |
worasuchad | 0:df6e371d8665 | 72 | { |
worasuchad | 3:91e508c05dca | 73 | #if(HORMONE) |
worasuchad | 3:91e508c05dca | 74 | downPreProc = (G2 + G3) / 2.00f; |
worasuchad | 3:91e508c05dca | 75 | |
worasuchad | 3:91e508c05dca | 76 | // hormone gland // |
worasuchad | 3:91e508c05dca | 77 | cgDown = tanh( (0.3f * downPreProc) + (0.5f * cgDownPrev) ); |
worasuchad | 3:91e508c05dca | 78 | cgDownPrev = cgDown; |
worasuchad | 3:91e508c05dca | 79 | |
worasuchad | 3:91e508c05dca | 80 | return (-25.00f * cgDown) + 107.50f; |
worasuchad | 3:91e508c05dca | 81 | #else |
worasuchad | 3:91e508c05dca | 82 | return 95.00f; |
worasuchad | 3:91e508c05dca | 83 | #endif |
worasuchad | 0:df6e371d8665 | 84 | } |
worasuchad | 0:df6e371d8665 | 85 | |
worasuchad | 0:df6e371d8665 | 86 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 87 | /* NAME : hormone receiver for servo up */ |
worasuchad | 0:df6e371d8665 | 88 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 89 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 90 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 91 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 92 | float hormone::hormoneRecUp(const float upDeg) |
worasuchad | 0:df6e371d8665 | 93 | { |
worasuchad | 0:df6e371d8665 | 94 | upDegree = upDeg; |
worasuchad | 0:df6e371d8665 | 95 | |
worasuchad | 0:df6e371d8665 | 96 | if(upDegree < 45.00f ) |
worasuchad | 0:df6e371d8665 | 97 | { |
worasuchad | 0:df6e371d8665 | 98 | upDegree = 45.00f; |
worasuchad | 0:df6e371d8665 | 99 | } |
worasuchad | 0:df6e371d8665 | 100 | else if(upDegree > 75.00f) |
worasuchad | 0:df6e371d8665 | 101 | { |
worasuchad | 0:df6e371d8665 | 102 | upDegree = 75.00f; |
worasuchad | 0:df6e371d8665 | 103 | } |
worasuchad | 0:df6e371d8665 | 104 | |
worasuchad | 0:df6e371d8665 | 105 | return upDegree; |
worasuchad | 0:df6e371d8665 | 106 | } |
worasuchad | 0:df6e371d8665 | 107 | |
worasuchad | 0:df6e371d8665 | 108 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 109 | /* NAME : hormone receiver for servo down */ |
worasuchad | 0:df6e371d8665 | 110 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 111 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 112 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 113 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 114 | float hormone::hormoneRecDown(const float downDeg) |
worasuchad | 0:df6e371d8665 | 115 | { |
worasuchad | 0:df6e371d8665 | 116 | //downDegree = 90.00f*(1.00f-(0.06f*cgDown)); |
worasuchad | 0:df6e371d8665 | 117 | downDegree = downDeg; |
worasuchad | 0:df6e371d8665 | 118 | |
worasuchad | 0:df6e371d8665 | 119 | if(downDegree < 90.00f) |
worasuchad | 0:df6e371d8665 | 120 | { |
worasuchad | 0:df6e371d8665 | 121 | downDegree = 90.00f; |
worasuchad | 0:df6e371d8665 | 122 | } |
worasuchad | 0:df6e371d8665 | 123 | else if(downDegree > 100.00f) |
worasuchad | 0:df6e371d8665 | 124 | { |
worasuchad | 0:df6e371d8665 | 125 | downDegree = 100.00f; |
worasuchad | 0:df6e371d8665 | 126 | } |
worasuchad | 0:df6e371d8665 | 127 | |
worasuchad | 0:df6e371d8665 | 128 | return downDegree; |
worasuchad | 0:df6e371d8665 | 129 | } |
worasuchad | 0:df6e371d8665 | 130 | /*--------------------------------------------------------------------*/ |