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