worasuchad haomachai / hormone

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?

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