Hormone for optimizing COT
Dependents: TurtleBot_v01 TurtleBot_with_AHS
hormone.cpp@0:df6e371d8665, 2018-08-13 (annotated)
- Committer:
- worasuchad
- Date:
- Mon Aug 13 11:33:22 2018 +0000
- Revision:
- 0:df6e371d8665
- Child:
- 1:43249c4dae57
hormone v.1 for optimizing COT
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 | 0:df6e371d8665 | 12 | upDegree = 45.00; downDegree = 95.00; |
worasuchad | 0:df6e371d8665 | 13 | cgUp = 45.00; cgDown = 95; |
worasuchad | 0:df6e371d8665 | 14 | cgUpPrev = 0.00; cgDownPrev = 0.00; |
worasuchad | 0:df6e371d8665 | 15 | upPreProc = 0.0; downPreProc = 0.0; |
worasuchad | 0:df6e371d8665 | 16 | } |
worasuchad | 0:df6e371d8665 | 17 | |
worasuchad | 0:df6e371d8665 | 18 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 19 | /* NAME : hormone concentration */ |
worasuchad | 0:df6e371d8665 | 20 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 21 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 22 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 23 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 24 | float hormone::upHG(const float G2,const float G4 ) |
worasuchad | 0:df6e371d8665 | 25 | { |
worasuchad | 0:df6e371d8665 | 26 | |
worasuchad | 0:df6e371d8665 | 27 | upPreProc = (G2 + G4) / 2; |
worasuchad | 0:df6e371d8665 | 28 | |
worasuchad | 0:df6e371d8665 | 29 | // hormone gland // |
worasuchad | 0:df6e371d8665 | 30 | cgUp = tanh( (0.7f * upPreProc) + (0.3f * cgUpPrev) ); |
worasuchad | 0:df6e371d8665 | 31 | cgUpPrev = cgUp; |
worasuchad | 0:df6e371d8665 | 32 | |
worasuchad | 0:df6e371d8665 | 33 | return 75.00f * cgUp; |
worasuchad | 0:df6e371d8665 | 34 | } |
worasuchad | 0:df6e371d8665 | 35 | |
worasuchad | 0:df6e371d8665 | 36 | |
worasuchad | 0:df6e371d8665 | 37 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 38 | /* NAME : hormone concentration for servo down */ |
worasuchad | 0:df6e371d8665 | 39 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 40 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 41 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 42 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 43 | float hormone::downHG(const float G2,const float G3 ) |
worasuchad | 0:df6e371d8665 | 44 | { |
worasuchad | 0:df6e371d8665 | 45 | |
worasuchad | 0:df6e371d8665 | 46 | downPreProc = (G2 + G3) / 2; |
worasuchad | 0:df6e371d8665 | 47 | |
worasuchad | 0:df6e371d8665 | 48 | // hormone gland // |
worasuchad | 0:df6e371d8665 | 49 | cgDown = tanh( (0.3f * downPreProc) + (0.5f * cgDownPrev) ); |
worasuchad | 0:df6e371d8665 | 50 | cgDownPrev = cgDown; |
worasuchad | 0:df6e371d8665 | 51 | |
worasuchad | 0:df6e371d8665 | 52 | return (-25.00f * cgDown) + 107.5f; |
worasuchad | 0:df6e371d8665 | 53 | } |
worasuchad | 0:df6e371d8665 | 54 | |
worasuchad | 0:df6e371d8665 | 55 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 56 | /* NAME : hormone receiver for servo up */ |
worasuchad | 0:df6e371d8665 | 57 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 58 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 59 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 60 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 61 | float hormone::hormoneRecUp(const float upDeg) |
worasuchad | 0:df6e371d8665 | 62 | { |
worasuchad | 0:df6e371d8665 | 63 | upDegree = upDeg; |
worasuchad | 0:df6e371d8665 | 64 | |
worasuchad | 0:df6e371d8665 | 65 | if(upDegree < 45.00f ) |
worasuchad | 0:df6e371d8665 | 66 | { |
worasuchad | 0:df6e371d8665 | 67 | upDegree = 45.00f; |
worasuchad | 0:df6e371d8665 | 68 | } |
worasuchad | 0:df6e371d8665 | 69 | else if(upDegree > 75.00f) |
worasuchad | 0:df6e371d8665 | 70 | { |
worasuchad | 0:df6e371d8665 | 71 | upDegree = 75.00f; |
worasuchad | 0:df6e371d8665 | 72 | } |
worasuchad | 0:df6e371d8665 | 73 | |
worasuchad | 0:df6e371d8665 | 74 | return upDegree; |
worasuchad | 0:df6e371d8665 | 75 | } |
worasuchad | 0:df6e371d8665 | 76 | |
worasuchad | 0:df6e371d8665 | 77 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:df6e371d8665 | 78 | /* NAME : hormone receiver for servo down */ |
worasuchad | 0:df6e371d8665 | 79 | /* PARAMETERS : - */ |
worasuchad | 0:df6e371d8665 | 80 | /* RETURN VALUE : - */ |
worasuchad | 0:df6e371d8665 | 81 | /* DESCRIPTION : - */ |
worasuchad | 0:df6e371d8665 | 82 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:df6e371d8665 | 83 | float hormone::hormoneRecDown(const float downDeg) |
worasuchad | 0:df6e371d8665 | 84 | { |
worasuchad | 0:df6e371d8665 | 85 | //downDegree = 90.00f*(1.00f-(0.06f*cgDown)); |
worasuchad | 0:df6e371d8665 | 86 | downDegree = downDeg; |
worasuchad | 0:df6e371d8665 | 87 | |
worasuchad | 0:df6e371d8665 | 88 | if(downDegree < 90.00f) |
worasuchad | 0:df6e371d8665 | 89 | { |
worasuchad | 0:df6e371d8665 | 90 | downDegree = 90.00f; |
worasuchad | 0:df6e371d8665 | 91 | } |
worasuchad | 0:df6e371d8665 | 92 | else if(downDegree > 100.00f) |
worasuchad | 0:df6e371d8665 | 93 | { |
worasuchad | 0:df6e371d8665 | 94 | downDegree = 100.00f; |
worasuchad | 0:df6e371d8665 | 95 | } |
worasuchad | 0:df6e371d8665 | 96 | |
worasuchad | 0:df6e371d8665 | 97 | return downDegree; |
worasuchad | 0:df6e371d8665 | 98 | } |
worasuchad | 0:df6e371d8665 | 99 | /*--------------------------------------------------------------------*/ |