Hormone for optimizing COT
Dependents: TurtleBot_v01 TurtleBot_with_AHS
hormone.cpp@4:0dfa16b90d92, 2019-11-20 (annotated)
- Committer:
- worasuchad
- Date:
- Wed Nov 20 06:50:25 2019 +0000
- Revision:
- 4:0dfa16b90d92
- Parent:
- 3:91e508c05dca
define 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 | 4:0dfa16b90d92 | 4 | #define HORMONE 0 // do you want to use hormone (1) or not (0) |
worasuchad | 4:0dfa16b90d92 | 5 | |
worasuchad | 4:0dfa16b90d92 | 6 | // gait configuration when hormone is disable |
worasuchad | 4:0dfa16b90d92 | 7 | #define GAITCONFIG 0 // 0-default, 1-flat, 2-gravel, 3-pebble terrain |
worasuchad | 3:91e508c05dca | 8 | |
worasuchad | 0:df6e371d8665 | 9 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 10 | /* NAME : constructor */ |
worasuchad | 0:df6e371d8665 | 11 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 12 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 13 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 14 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 15 | hormone::hormone() |
worasuchad | 0:df6e371d8665 | 16 | { |
worasuchad | 4:0dfa16b90d92 | 17 | #if(GAITCONFIG == 0) |
worasuchad | 4:0dfa16b90d92 | 18 | // default config // |
worasuchad | 4:0dfa16b90d92 | 19 | upDegree = 45.00; downDegree = 90.00; |
worasuchad | 4:0dfa16b90d92 | 20 | cgUp = 45.00; cgDown = 90.00; |
worasuchad | 4:0dfa16b90d92 | 21 | //cgUpPrev = 0.6; cgDownPrev = 0.7; |
worasuchad | 4:0dfa16b90d92 | 22 | #endif |
worasuchad | 4:0dfa16b90d92 | 23 | |
worasuchad | 4:0dfa16b90d92 | 24 | #if(GAITCONFIG == 1) |
worasuchad | 4:0dfa16b90d92 | 25 | // flat config // |
worasuchad | 4:0dfa16b90d92 | 26 | upDegree = 45.00; downDegree = 95.00; |
worasuchad | 4:0dfa16b90d92 | 27 | cgUp = 45.00; cgDown = 95; |
worasuchad | 4:0dfa16b90d92 | 28 | //cgUpPrev = 0.6; cgDownPrev = 0.5; |
worasuchad | 4:0dfa16b90d92 | 29 | #endif |
worasuchad | 3:91e508c05dca | 30 | |
worasuchad | 4:0dfa16b90d92 | 31 | #if(GAITCONFIG == 2) |
worasuchad | 4:0dfa16b90d92 | 32 | // small config // |
worasuchad | 4:0dfa16b90d92 | 33 | upDegree = 60.00; downDegree = 100.00; |
worasuchad | 4:0dfa16b90d92 | 34 | cgUp = 60.00; cgDown = 100; |
worasuchad | 4:0dfa16b90d92 | 35 | //cgUpPrev = 0.8; cgDownPrev = 0.3; |
worasuchad | 4:0dfa16b90d92 | 36 | #endif |
worasuchad | 4:0dfa16b90d92 | 37 | |
worasuchad | 4:0dfa16b90d92 | 38 | #if(GAITCONFIG == 3) |
worasuchad | 4:0dfa16b90d92 | 39 | // big config // |
worasuchad | 4:0dfa16b90d92 | 40 | upDegree = 75.00; downDegree = 90.00; |
worasuchad | 4:0dfa16b90d92 | 41 | cgUp = 75.00; cgDown = 90; |
worasuchad | 4:0dfa16b90d92 | 42 | //cgUpPrev = 1.0; cgDownPrev = 0.7; |
worasuchad | 4:0dfa16b90d92 | 43 | #endif |
worasuchad | 4:0dfa16b90d92 | 44 | |
worasuchad | 4:0dfa16b90d92 | 45 | cgUpPrev = 0.0; cgDownPrev = 0.0; |
worasuchad | 3:91e508c05dca | 46 | upPreProc = 0.0; downPreProc = 0.0; |
worasuchad | 0:df6e371d8665 | 47 | } |
worasuchad | 0:df6e371d8665 | 48 | |
worasuchad | 0:df6e371d8665 | 49 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 50 | /* NAME : hormone concentration */ |
worasuchad | 0:df6e371d8665 | 51 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 52 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 53 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 54 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 55 | float hormone::upHG(const float G2,const float G4 ) |
worasuchad | 0:df6e371d8665 | 56 | { |
worasuchad | 3:91e508c05dca | 57 | #if(HORMONE) |
worasuchad | 3:91e508c05dca | 58 | upPreProc = (G2 + G4) / 2.00f; |
worasuchad | 3:91e508c05dca | 59 | |
worasuchad | 3:91e508c05dca | 60 | // hormone gland // |
worasuchad | 3:91e508c05dca | 61 | cgUp = tanh( (0.7f * upPreProc) + (0.3f * cgUpPrev) ); |
worasuchad | 3:91e508c05dca | 62 | cgUpPrev = cgUp; |
worasuchad | 3:91e508c05dca | 63 | |
worasuchad | 3:91e508c05dca | 64 | return 75 * cgUp; |
worasuchad | 3:91e508c05dca | 65 | #else |
worasuchad | 4:0dfa16b90d92 | 66 | #if(GAITCONFIG == 0 || GAITCONFIG == 1 ) |
worasuchad | 4:0dfa16b90d92 | 67 | return 45.00f; |
worasuchad | 4:0dfa16b90d92 | 68 | #endif |
worasuchad | 4:0dfa16b90d92 | 69 | |
worasuchad | 4:0dfa16b90d92 | 70 | #if(GAITCONFIG == 2) |
worasuchad | 4:0dfa16b90d92 | 71 | return 60.00f; |
worasuchad | 4:0dfa16b90d92 | 72 | #endif |
worasuchad | 4:0dfa16b90d92 | 73 | |
worasuchad | 4:0dfa16b90d92 | 74 | #if(GAITCONFIG == 3) |
worasuchad | 4:0dfa16b90d92 | 75 | return 75.00f; |
worasuchad | 4:0dfa16b90d92 | 76 | #endif |
worasuchad | 3:91e508c05dca | 77 | #endif |
worasuchad | 0:df6e371d8665 | 78 | } |
worasuchad | 0:df6e371d8665 | 79 | |
worasuchad | 0:df6e371d8665 | 80 | |
worasuchad | 0:df6e371d8665 | 81 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 82 | /* NAME : hormone concentration for servo down */ |
worasuchad | 0:df6e371d8665 | 83 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 84 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 85 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 86 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 87 | float hormone::downHG(const float G2,const float G3 ) |
worasuchad | 0:df6e371d8665 | 88 | { |
worasuchad | 3:91e508c05dca | 89 | #if(HORMONE) |
worasuchad | 3:91e508c05dca | 90 | downPreProc = (G2 + G3) / 2.00f; |
worasuchad | 3:91e508c05dca | 91 | |
worasuchad | 3:91e508c05dca | 92 | // hormone gland // |
worasuchad | 3:91e508c05dca | 93 | cgDown = tanh( (0.3f * downPreProc) + (0.5f * cgDownPrev) ); |
worasuchad | 3:91e508c05dca | 94 | cgDownPrev = cgDown; |
worasuchad | 3:91e508c05dca | 95 | |
worasuchad | 3:91e508c05dca | 96 | return (-25.00f * cgDown) + 107.50f; |
worasuchad | 3:91e508c05dca | 97 | #else |
worasuchad | 4:0dfa16b90d92 | 98 | #if(GAITCONFIG == 0 || GAITCONFIG == 3 ) |
worasuchad | 4:0dfa16b90d92 | 99 | return 90.00f; |
worasuchad | 4:0dfa16b90d92 | 100 | #endif |
worasuchad | 4:0dfa16b90d92 | 101 | |
worasuchad | 4:0dfa16b90d92 | 102 | #if(GAITCONFIG == 1) |
worasuchad | 4:0dfa16b90d92 | 103 | return 95.00f; |
worasuchad | 4:0dfa16b90d92 | 104 | #endif |
worasuchad | 4:0dfa16b90d92 | 105 | |
worasuchad | 4:0dfa16b90d92 | 106 | #if(GAITCONFIG == 2) |
worasuchad | 4:0dfa16b90d92 | 107 | return 100.00f; |
worasuchad | 4:0dfa16b90d92 | 108 | #endif |
worasuchad | 3:91e508c05dca | 109 | #endif |
worasuchad | 0:df6e371d8665 | 110 | } |
worasuchad | 0:df6e371d8665 | 111 | |
worasuchad | 0:df6e371d8665 | 112 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 113 | /* NAME : hormone receiver for servo up */ |
worasuchad | 0:df6e371d8665 | 114 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 115 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 116 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 117 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 118 | float hormone::hormoneRecUp(const float upDeg) |
worasuchad | 0:df6e371d8665 | 119 | { |
worasuchad | 0:df6e371d8665 | 120 | upDegree = upDeg; |
worasuchad | 0:df6e371d8665 | 121 | |
worasuchad | 0:df6e371d8665 | 122 | if(upDegree < 45.00f ) |
worasuchad | 0:df6e371d8665 | 123 | { |
worasuchad | 0:df6e371d8665 | 124 | upDegree = 45.00f; |
worasuchad | 0:df6e371d8665 | 125 | } |
worasuchad | 0:df6e371d8665 | 126 | else if(upDegree > 75.00f) |
worasuchad | 0:df6e371d8665 | 127 | { |
worasuchad | 0:df6e371d8665 | 128 | upDegree = 75.00f; |
worasuchad | 0:df6e371d8665 | 129 | } |
worasuchad | 0:df6e371d8665 | 130 | |
worasuchad | 0:df6e371d8665 | 131 | return upDegree; |
worasuchad | 0:df6e371d8665 | 132 | } |
worasuchad | 0:df6e371d8665 | 133 | |
worasuchad | 0:df6e371d8665 | 134 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 135 | /* NAME : hormone receiver for servo down */ |
worasuchad | 0:df6e371d8665 | 136 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 137 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 138 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 139 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 140 | float hormone::hormoneRecDown(const float downDeg) |
worasuchad | 0:df6e371d8665 | 141 | { |
worasuchad | 0:df6e371d8665 | 142 | //downDegree = 90.00f*(1.00f-(0.06f*cgDown)); |
worasuchad | 0:df6e371d8665 | 143 | downDegree = downDeg; |
worasuchad | 0:df6e371d8665 | 144 | |
worasuchad | 0:df6e371d8665 | 145 | if(downDegree < 90.00f) |
worasuchad | 0:df6e371d8665 | 146 | { |
worasuchad | 0:df6e371d8665 | 147 | downDegree = 90.00f; |
worasuchad | 0:df6e371d8665 | 148 | } |
worasuchad | 0:df6e371d8665 | 149 | else if(downDegree > 100.00f) |
worasuchad | 0:df6e371d8665 | 150 | { |
worasuchad | 0:df6e371d8665 | 151 | downDegree = 100.00f; |
worasuchad | 0:df6e371d8665 | 152 | } |
worasuchad | 0:df6e371d8665 | 153 | |
worasuchad | 0:df6e371d8665 | 154 | return downDegree; |
worasuchad | 0:df6e371d8665 | 155 | } |
worasuchad | 0:df6e371d8665 | 156 | /*--------------------------------------------------------------------*/ |