worasuchad haomachai / hormone

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?

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