worasuchad haomachai / hormone

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?

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