Hormone for optimizing COT
Dependents: TurtleBot_v01 TurtleBot_with_AHS
hormone.cpp@2:6da425fac1f7, 2018-08-24 (annotated)
- Committer:
- worasuchad
- Date:
- Fri Aug 24 10:55:36 2018 +0000
- Revision:
- 2:6da425fac1f7
- Parent:
- 1:43249c4dae57
- Child:
- 3:91e508c05dca
experiment done
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 | 0:df6e371d8665 | 4 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 5 | /* NAME : constructor */ |
worasuchad | 0:df6e371d8665 | 6 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 7 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 8 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 9 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 10 | hormone::hormone() |
worasuchad | 0:df6e371d8665 | 11 | { |
worasuchad | 2:6da425fac1f7 | 12 | /* |
worasuchad | 2:6da425fac1f7 | 13 | // flat config // |
worasuchad | 0:df6e371d8665 | 14 | upDegree = 45.00; downDegree = 95.00; |
worasuchad | 0:df6e371d8665 | 15 | cgUp = 45.00; cgDown = 95; |
worasuchad | 2:6da425fac1f7 | 16 | cgUpPrev = 0.6; |
worasuchad | 2:6da425fac1f7 | 17 | cgDownPrev = 0.5; |
worasuchad | 2:6da425fac1f7 | 18 | */ |
worasuchad | 2:6da425fac1f7 | 19 | // small config // |
worasuchad | 2:6da425fac1f7 | 20 | upDegree = 60.00; downDegree = 100.00; |
worasuchad | 2:6da425fac1f7 | 21 | cgUp = 60.00; cgDown = 100; |
worasuchad | 2:6da425fac1f7 | 22 | cgUpPrev = 0.8; |
worasuchad | 2:6da425fac1f7 | 23 | cgDownPrev = 0.3; |
worasuchad | 2:6da425fac1f7 | 24 | |
worasuchad | 2:6da425fac1f7 | 25 | /* |
worasuchad | 2:6da425fac1f7 | 26 | // big config // |
worasuchad | 2:6da425fac1f7 | 27 | upDegree = 75.00; downDegree = 90.00; |
worasuchad | 2:6da425fac1f7 | 28 | cgUp = 75.00; cgDown = 90; |
worasuchad | 2:6da425fac1f7 | 29 | cgUpPrev = 1.0; |
worasuchad | 2:6da425fac1f7 | 30 | cgDownPrev = 0.7; |
worasuchad | 2:6da425fac1f7 | 31 | */ |
worasuchad | 2:6da425fac1f7 | 32 | //upPreProc = 0.0; downPreProc = 0.0; |
worasuchad | 0:df6e371d8665 | 33 | } |
worasuchad | 0:df6e371d8665 | 34 | |
worasuchad | 0:df6e371d8665 | 35 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 36 | /* NAME : hormone concentration */ |
worasuchad | 0:df6e371d8665 | 37 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 38 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 39 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 40 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 41 | float hormone::upHG(const float G2,const float G4 ) |
worasuchad | 0:df6e371d8665 | 42 | { |
worasuchad | 2:6da425fac1f7 | 43 | /* |
worasuchad | 2:6da425fac1f7 | 44 | upPreProc = (G2 + G4) / 2.00f; |
worasuchad | 0:df6e371d8665 | 45 | |
worasuchad | 0:df6e371d8665 | 46 | // hormone gland // |
worasuchad | 0:df6e371d8665 | 47 | cgUp = tanh( (0.7f * upPreProc) + (0.3f * cgUpPrev) ); |
worasuchad | 2:6da425fac1f7 | 48 | //cgUp = tanh( (0.3f * upPreProc) + (0.5f * cgUpPrev) ); |
worasuchad | 0:df6e371d8665 | 49 | cgUpPrev = cgUp; |
worasuchad | 0:df6e371d8665 | 50 | |
worasuchad | 2:6da425fac1f7 | 51 | return 75 * cgUp; |
worasuchad | 2:6da425fac1f7 | 52 | //return (50.00f * cgUp) + 30.00f; |
worasuchad | 2:6da425fac1f7 | 53 | */ |
worasuchad | 2:6da425fac1f7 | 54 | return 60.00f; |
worasuchad | 0:df6e371d8665 | 55 | } |
worasuchad | 0:df6e371d8665 | 56 | |
worasuchad | 0:df6e371d8665 | 57 | |
worasuchad | 0:df6e371d8665 | 58 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 59 | /* NAME : hormone concentration for servo down */ |
worasuchad | 0:df6e371d8665 | 60 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 61 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 62 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 63 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 64 | float hormone::downHG(const float G2,const float G3 ) |
worasuchad | 0:df6e371d8665 | 65 | { |
worasuchad | 2:6da425fac1f7 | 66 | /* |
worasuchad | 2:6da425fac1f7 | 67 | downPreProc = (G2 + G3) / 2.00f; |
worasuchad | 0:df6e371d8665 | 68 | |
worasuchad | 0:df6e371d8665 | 69 | // hormone gland // |
worasuchad | 0:df6e371d8665 | 70 | cgDown = tanh( (0.3f * downPreProc) + (0.5f * cgDownPrev) ); |
worasuchad | 0:df6e371d8665 | 71 | cgDownPrev = cgDown; |
worasuchad | 0:df6e371d8665 | 72 | |
worasuchad | 1:43249c4dae57 | 73 | return (-25.00f * cgDown) + 107.50f; |
worasuchad | 2:6da425fac1f7 | 74 | */ |
worasuchad | 2:6da425fac1f7 | 75 | return 100.00f; |
worasuchad | 0:df6e371d8665 | 76 | } |
worasuchad | 0:df6e371d8665 | 77 | |
worasuchad | 0:df6e371d8665 | 78 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 79 | /* NAME : hormone receiver for servo up */ |
worasuchad | 0:df6e371d8665 | 80 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 81 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 82 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 83 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 84 | float hormone::hormoneRecUp(const float upDeg) |
worasuchad | 0:df6e371d8665 | 85 | { |
worasuchad | 0:df6e371d8665 | 86 | upDegree = upDeg; |
worasuchad | 0:df6e371d8665 | 87 | |
worasuchad | 0:df6e371d8665 | 88 | if(upDegree < 45.00f ) |
worasuchad | 0:df6e371d8665 | 89 | { |
worasuchad | 0:df6e371d8665 | 90 | upDegree = 45.00f; |
worasuchad | 0:df6e371d8665 | 91 | } |
worasuchad | 0:df6e371d8665 | 92 | else if(upDegree > 75.00f) |
worasuchad | 0:df6e371d8665 | 93 | { |
worasuchad | 0:df6e371d8665 | 94 | upDegree = 75.00f; |
worasuchad | 0:df6e371d8665 | 95 | } |
worasuchad | 0:df6e371d8665 | 96 | |
worasuchad | 0:df6e371d8665 | 97 | return upDegree; |
worasuchad | 0:df6e371d8665 | 98 | } |
worasuchad | 0:df6e371d8665 | 99 | |
worasuchad | 0:df6e371d8665 | 100 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 101 | /* NAME : hormone receiver for servo down */ |
worasuchad | 0:df6e371d8665 | 102 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 103 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 104 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 105 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 106 | float hormone::hormoneRecDown(const float downDeg) |
worasuchad | 0:df6e371d8665 | 107 | { |
worasuchad | 0:df6e371d8665 | 108 | //downDegree = 90.00f*(1.00f-(0.06f*cgDown)); |
worasuchad | 0:df6e371d8665 | 109 | downDegree = downDeg; |
worasuchad | 0:df6e371d8665 | 110 | |
worasuchad | 0:df6e371d8665 | 111 | if(downDegree < 90.00f) |
worasuchad | 0:df6e371d8665 | 112 | { |
worasuchad | 0:df6e371d8665 | 113 | downDegree = 90.00f; |
worasuchad | 0:df6e371d8665 | 114 | } |
worasuchad | 0:df6e371d8665 | 115 | else if(downDegree > 100.00f) |
worasuchad | 0:df6e371d8665 | 116 | { |
worasuchad | 0:df6e371d8665 | 117 | downDegree = 100.00f; |
worasuchad | 0:df6e371d8665 | 118 | } |
worasuchad | 0:df6e371d8665 | 119 | |
worasuchad | 0:df6e371d8665 | 120 | return downDegree; |
worasuchad | 0:df6e371d8665 | 121 | } |
worasuchad | 0:df6e371d8665 | 122 | /*--------------------------------------------------------------------*/ |