Wiselib: wiselib.testing/intermediate/robot/controlled

Wiselib
00001 // vim: set noexpandtab ts=3 sw=3:
00002 
00003 #ifndef CONTROLLED_MOTION_H
00004 #define CONTROLLED_MOTION_H
00005 
00006 #include "util/standalone_math.h"
00007 
00008 namespace wiselib {
00009    
00017    template<
00018       typename OsModel_P,
00019       typename Robot_P,
00020       typename Odometer_P = Robot_P,
00021       typename Math_P = StandaloneMath<OsModel_P>,
00022       typename OsModel_P::size_t MAX_RECEIVERS = 16
00023    >
00024    class ControlledMotion {
00025       public:
00026          typedef OsModel_P OsModel;
00027          typedef Robot_P Robot;
00028          typedef Odometer_P Odometer;
00029          typedef Math_P Math;
00030          
00031          typedef ControlledMotion<OsModel, Robot, Odometer, Math, MAX_RECEIVERS> self_type;
00032          typedef self_type* self_pointer_t;
00033          
00034          typedef typename Odometer::distance_t distance_t;
00035          typedef typename Odometer::angle_t angle_t;
00036          
00037          typedef typename Robot::velocity_t velocity_t;
00038          typedef typename Robot::angular_velocity_t angular_velocity_t;
00039          
00040          typedef delegate0<void> stopped_delegate_t;
00041          typedef vector_static<OsModel, stopped_delegate_t, MAX_RECEIVERS> stopped_delegates_t;
00042          
00043          enum ErrorCodes {
00044             SUCCESS = OsModel::SUCCESS, ERR_UNSPEC = OsModel::ERR_UNSPEC
00045          };
00046          
00047          inline int init();
00048          inline int init(Robot&);
00049          inline int init(Robot&, Odometer&);
00050          
00051          inline int destruct();
00052          
00053          inline int move_distance(distance_t d, velocity_t v = Robot::PRECISE_VELOCITY);
00054          inline int turn_about(angle_t a, angular_velocity_t v = Robot::PRECISE_ANGULAR_VELOCITY);
00055          inline int turn_to(angle_t a, angular_velocity_t v = Robot::PRECISE_ANGULAR_VELOCITY);
00056          
00057          void on_state_change(int);
00058          
00059          template<typename T, void (T::*TMethod)(void)>
00060          inline int reg_stopped_callback(T*);
00061          inline int unreg_stopped_callback(typename OsModel::size_t);
00062          
00063          Robot& robot() { return *robot_; }
00064          
00065       private:
00066          enum Mode { NONE, ANGLE, DISTANCE };
00067          
00068          typename Robot::self_pointer_t robot_;
00069          typename Odometer::self_pointer_t odometer_;
00070          
00071          Mode mode_;
00072          distance_t distance_, target_distance_;
00073          angle_t angle_, target_angle_;
00074          bool angle_increasing_, distance_increasing_;
00075          
00076          stopped_delegates_t stopped_callbacks_;
00077          
00078          int on_state_change_idx_;
00079    }; // ControlledMotion
00080    
00081    template<typename OsModel_P, typename Robot_P, typename Odometer_P, typename Math_P, typename OsModel_P::size_t MAX_RECEIVERS>
00082    int
00083    ControlledMotion<OsModel_P, Robot_P, Odometer_P, Math_P, MAX_RECEIVERS>::
00084 	init() {
00085       destruct();
00086       init(*robot_, *odometer_);
00087       return SUCCESS;
00088    }
00089    
00090    template<typename OsModel_P, typename Robot_P, typename Odometer_P, typename Math_P, typename OsModel_P::size_t MAX_RECEIVERS>
00091    int
00092    ControlledMotion<OsModel_P, Robot_P, Odometer_P, Math_P, MAX_RECEIVERS>::
00093 	init(Robot_P& robot) {
00094       mode_ = NONE;
00095       robot_ = &robot;
00096       odometer_ = &robot;
00097       on_state_change_idx_ = odometer_->template register_state_callback<self_type, &self_type::on_state_change>(this);
00098       return SUCCESS;
00099    }
00100    
00101    template<typename OsModel_P, typename Robot_P, typename Odometer_P, typename Math_P, typename OsModel_P::size_t MAX_RECEIVERS>
00102    int
00103    ControlledMotion<OsModel_P, Robot_P, Odometer_P, Math_P, MAX_RECEIVERS>::
00104 	init(Robot_P& robot, Odometer_P& odometer) {
00105       mode_ = NONE;
00106       robot_ = &robot;
00107       odometer_ = &odometer;
00108       on_state_change_idx_ = odometer_->template register_state_callback<self_type, &self_type::on_state_change>(this);
00109       return SUCCESS;
00110    }
00111    
00112    template<typename OsModel_P, typename Robot_P, typename Odometer_P, typename Math_P, typename OsModel_P::size_t MAX_RECEIVERS>
00113    int
00114    ControlledMotion<OsModel_P, Robot_P, Odometer_P, Math_P, MAX_RECEIVERS>::
00115 	destruct() {
00116       odometer_.unregister_state_callback(on_state_change_idx_);
00117       return SUCCESS;
00118    }
00119    
00120    template<typename OsModel_P, typename Robot_P, typename Odometer_P, typename Math_P, typename OsModel_P::size_t MAX_RECEIVERS>
00121    int
00122    ControlledMotion<OsModel_P, Robot_P, Odometer_P, Math_P, MAX_RECEIVERS>::
00123 	move_distance(
00124          ControlledMotion<OsModel_P, Robot_P, Odometer_P, Math_P, MAX_RECEIVERS>::distance_t distance,
00125          ControlledMotion<OsModel_P, Robot_P, Odometer_P, Math_P, MAX_RECEIVERS>::velocity_t velocity
00126    ) {
00127       mode_ = DISTANCE;
00128       distance_ = robot_->distance();
00129       target_distance_ = distance_ + distance;
00130       distance_increasing_ = (target_distance_ >= distance_);
00131       robot_->move(distance_increasing_ ? velocity : -velocity);
00132       return SUCCESS;
00133    }
00134    
00135    template<typename OsModel_P, typename Robot_P, typename Odometer_P, typename Math_P, typename OsModel_P::size_t MAX_RECEIVERS>
00136    int
00137    ControlledMotion<OsModel_P, Robot_P, Odometer_P, Math_P, MAX_RECEIVERS>::
00138 	turn_about(
00139          ControlledMotion<OsModel_P, Robot_P, Odometer_P, Math_P, MAX_RECEIVERS>::angle_t angle,
00140          ControlledMotion<OsModel_P, Robot_P, Odometer_P, Math_P, MAX_RECEIVERS>::angular_velocity_t velocity
00141    ) {
00142       return turn_to(angle_ + angle, velocity);
00143    }
00144    
00145    template<typename OsModel_P, typename Robot_P, typename Odometer_P, typename Math_P, typename OsModel_P::size_t MAX_RECEIVERS>
00146    int
00147    ControlledMotion<OsModel_P, Robot_P, Odometer_P, Math_P, MAX_RECEIVERS>::
00148 	turn_to(
00149          ControlledMotion<OsModel_P, Robot_P, Odometer_P, Math_P, MAX_RECEIVERS>::angle_t angle,
00150          ControlledMotion<OsModel_P, Robot_P, Odometer_P, Math_P, MAX_RECEIVERS>::angular_velocity_t velocity
00151    ) {
00152       mode_ = ANGLE;
00153       target_angle_ = angle;
00154       
00155       if(target_angle_ < angle_) {
00156          velocity = -velocity;
00157          angle_increasing_ = false;
00158       }
00159       else {
00160          angle_increasing_ = true;
00161       }
00162       
00163       robot_->turn(velocity);
00164       return SUCCESS;
00165    }
00166    
00167    template<typename OsModel_P, typename Robot_P, typename Odometer_P, typename Math_P, typename OsModel_P::size_t MAX_RECEIVERS>
00168    template<typename T, void (T::*TMethod)(void)>
00169    int
00170    ControlledMotion<OsModel_P, Robot_P, Odometer_P, Math_P, MAX_RECEIVERS>::
00171 	reg_stopped_callback(T* obj) {
00172       if(stopped_callbacks_.empty()) {
00173          stopped_callbacks_.assign(MAX_RECEIVERS, stopped_delegate_t());
00174       }
00175       
00176       for(typename OsModel::size_t i=0; i<stopped_callbacks_.size(); i++) {
00177          if(stopped_callbacks_[i] == stopped_delegate_t()) {
00178             stopped_callbacks_[i] = stopped_delegate_t::template from_method<T, TMethod>(obj);
00179             return i;
00180          }
00181       }
00182       return -1;
00183    }
00184    
00185    template<typename OsModel_P, typename Robot_P, typename Odometer_P, typename Math_P, typename OsModel_P::size_t MAX_RECEIVERS>
00186    int
00187    ControlledMotion<OsModel_P, Robot_P, Odometer_P, Math_P, MAX_RECEIVERS>::
00188 	unreg_stopped_callback(typename OsModel_P::size_t idx) {
00189       stopped_callbacks_[idx] = stopped_delegate_t();
00190       return OsModel::SUCCESS;
00191    }
00192    
00193    template<typename OsModel_P, typename Robot_P, typename Odometer_P, typename Math_P, typename OsModel_P::size_t MAX_RECEIVERS>
00194    void
00195    ControlledMotion<OsModel_P, Robot_P, Odometer_P, Math_P, MAX_RECEIVERS>::
00196 	on_state_change(int state) {
00197       if(state == Odometer::DATA_AVAILABLE) {
00198          angle_ = odometer_->angle();
00199          switch(mode_) {
00200             case NONE:
00201                break;
00202                
00203             case ANGLE:
00204                if((angle_increasing_ && (angle_ >= target_angle_)) ||
00205                   ((!angle_increasing_ && (angle_ <= target_angle_)))
00206                ) {
00207                   robot_->stop();
00208                }
00209                break;
00210                
00211             case DISTANCE:
00212                distance_ = odometer_->distance();
00213                if((distance_increasing_ && (distance_ >= target_distance_)) ||
00214                   ((!distance_increasing_ && (distance_ <= target_distance_)))
00215                ) {
00216                   robot_->stop();
00217                }
00218                break;
00219          } // switch
00220       } // DATA_AVAILABLE
00221    } // on_state_change()
00222    
00223 }; // namespace
00224 
00225 #endif // CONTROLLED_MOTION_H
00226