worasuchad haomachai / hormone

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?

UserRevisionLine numberNew 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 /*--------------------------------------------------------------------*/