00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 
00021 
00022 
00023 
00024 
00025 #include "Teddy/Behaviour/Control.h"
00026 using namespace Teddy::MixIn;
00027 
00028 namespace Teddy     {
00029 namespace Behaviour {
00030 
00031 
00032  const unsigned long Control::RESERVED        = 0ul;
00033  const unsigned long Control::MORE            = 1ul;
00034  const unsigned long Control::LESS            = 2ul;
00035  const unsigned long Control::STOP            = 3ul;
00036  const unsigned long Control::ACTIVE          = 4ul;
00037  const unsigned long Control::INHIBIT         = 5ul;
00038  const unsigned long Control::DAMPEN_LINEAR   = 6ul;
00039  const unsigned long Control::DAMPEN_MULTIPLY = 7ul;
00040  const unsigned long Control::DAMPEN_MASTER   = 8ul;
00041                                                         
00042  const unsigned long Control::RESERVED_M        = (1ul<<Control::RESERVED       );
00043  const unsigned long Control::MORE_M            = (1ul<<Control::MORE           );
00044  const unsigned long Control::LESS_M            = (1ul<<Control::LESS           );
00045  const unsigned long Control::STOP_M            = (1ul<<Control::STOP           );
00046  const unsigned long Control::ACTIVE_M          = (1ul<<Control::ACTIVE         );
00047  const unsigned long Control::INHIBIT_M         = (1ul<<Control::INHIBIT        );
00048  const unsigned long Control::DAMPEN_LINEAR_M   = (1ul<<Control::DAMPEN_LINEAR  );
00049  const unsigned long Control::DAMPEN_MULTIPLY_M = (1ul<<Control::DAMPEN_MULTIPLY);
00050  const unsigned long Control::DAMPEN_MASTER_M   = (1ul<<Control::DAMPEN_MASTER  );
00051 
00052  const unsigned long Control::CLEAR_M =
00053     Control::MORE_M    |  
00054     Control::LESS_M    |  
00055     Control::STOP_M    |  
00056     Control::ACTIVE_M  |  
00057     Control::INHIBIT_M;
00058 
00059 
00061 Control::Control( double max_delta, double max, Options options, double dampen_const )
00062 :
00063 Options     (options),
00064 
00065 
00066 
00067 
00068 
00069 
00070 
00071 dampen_const(dampen_const),
00072 max_delta   (max_delta),
00073 min         (-max ),
00074 max         ( max ),
00075 c_delta     (0),
00076 c_value     (0)
00077 {
00078 }
00079 
00080 
00081 void Control::setValue( double value ){
00082     c_value = value;
00083 }
00084 
00085 
00086 double Control::getValue() const {
00087     return c_value;
00088 }
00089 
00090 
00091 double Control::getMin() const {
00092     return min;
00093 }
00094 
00095 
00096 double Control::getMax() const {
00097     return max;
00098 }
00099 
00100 
00101 double Control::getDelta() const {
00102     return max_delta;
00103 }
00104 
00105 
00106 double Control::getDampConst() const {
00107     return dampen_const;
00108 }
00109 
00110 
00111 void Control::setDelta( double max_delta ){
00112     this->max_delta = max_delta;
00113 }
00114 
00115 
00116 void Control::setDampConst( double dampen_const ){
00117     this->dampen_const = dampen_const;
00118 }
00119 
00120 
00121 void Control::clear(){
00122     disable( Control::CLEAR_M );
00123 
00124 
00125 
00126 
00127 
00128     c_delta   = 0;
00129     c_value   = 0;
00130 }
00131 
00132 
00134  void Control::tick(){
00135     static const double age = 10.0;
00136 
00137     if( isEnabled(Control::ACTIVE_M) && isDisabled(Control::INHIBIT_M) ){
00138         adjust( c_delta * age );
00139     }
00140     dampen();
00141 }
00142 
00143 
00145 void Control::adjust( double delta ){
00146     c_value += delta;
00147 
00148     if( c_value>max ){
00149         c_value = max;
00150     }else if( c_value<min ){
00151         c_value = min;
00152     }
00153 }
00154 
00155 
00157 void Control::dampen(){
00158     static const double age = 10.0;
00159 
00160     if( isEnabled(Control::DAMPEN_MULTIPLY_M) ){
00161 
00162         c_value *= dampen_const;
00163 
00164     }else if( isEnabled(Control::DAMPEN_LINEAR_M) && isDisabled(Control::ACTIVE_M) ){
00165 
00166         if( c_value > max_delta ){
00167             c_value -= max_delta*age; 
00168             if( c_value < max_delta ){
00169                 c_value = 0;
00170             }
00171         }else if( c_value < -max_delta ){
00172             c_value += max_delta*age;
00173             if( c_value > -max_delta ){
00174                 c_value = 0;
00175             }
00176         }else{
00177             c_value *= dampen_const;
00178         }
00179     }
00180 }
00181 
00182 
00184 void Control::more( bool apply ){
00185     if( apply ){
00186         enable( Control::MORE_M | Control::ACTIVE_M );
00187         c_delta  =  max_delta;
00188     }else{
00189         disable( Control::MORE_M );
00190         if( isEnabled(Control::LESS_M) ){
00191             c_delta  = -max_delta;
00192         }else{
00193             disable( Control::ACTIVE_M );
00194             c_delta  =  0;
00195         }
00196     }
00197 }
00198 
00199 
00201 void Control::less( bool apply ){
00202     if( apply ){
00203         enable( Control::LESS_M | Control::ACTIVE_M );
00204         c_delta  = -max_delta;
00205     }else{
00206         disable( Control::LESS_M );
00207         if( isEnabled(Control::MORE_M) ){
00208             c_delta  =  max_delta;
00209         }else{
00210             disable( Control::ACTIVE_M );
00211             c_delta  =  0;
00212         }
00213     }
00214 }
00215 
00216 
00218 void Control::stop( bool apply ){
00219     if( apply ){
00220         enable( Control::STOP_M );
00221         if( c_value > 0 ){
00222             c_delta  = -max_delta;
00223         }else if( c_value < 0 ){
00224             c_delta  =  max_delta;
00225         }
00226     }else{
00227         disable( Control::STOP_M );
00228         if( isEnabled(Control::LESS_M) && isDisabled(Control::MORE_M) ){
00229             c_delta = -max_delta;
00230         }else if( isEnabled(Control::MORE_M) && isDisabled(Control::LESS_M) ){
00231             c_delta =  max_delta;
00232         }
00233     }
00234 }
00235 
00236 
00237 };  
00238 };  
00239