Wiselib: wiselib.testing/intermediate/robot/direct

Wiselib
00001 // vim: set noexpandtab ts=3 sw=3:
00002 
00003 #ifndef DIRECT_MOTION_H
00004 #define DIRECT_MOTION_H
00005 
00006 #include "util/standalone_math.h"
00007 
00008 namespace wiselib {
00009    
00024    template<
00025       typename OsModel_P,
00026       typename Position_P,
00027       typename ControlledMotion_P,
00028       typename Math_P = StandaloneMath<OsModel_P>
00029    >
00030    class DirectMotion {
00031       public:
00032          typedef OsModel_P OsModel;
00033          typedef Position_P Position;
00034          typedef ControlledMotion_P ControlledMotion;
00035          typedef Math_P Math;
00036          typedef typename Position::position_t position_t;
00037          
00038          typedef DirectMotion<OsModel, Position, ControlledMotion, Math> self_type;
00039          typedef self_type* self_pointer_t;
00040          
00041          enum ErrorCodes {
00042             SUCCESS = OsModel::SUCCESS, ERR_UNSPEC = OsModel::ERR_UNSPEC
00043          };
00044          
00045          inline int init();
00046          inline int init(Position&, ControlledMotion&);
00047          inline int destruct();
00048          
00051          inline int move_to(const position_t&);
00052          
00053       private:
00054          typename Position::self_pointer_t position_;
00055          typename ControlledMotion::self_pointer_t controlled_motion_;
00056    };
00057    
00058    template<
00059       typename OsModel_P,
00060       typename Position_P,
00061       typename ControlledMotion_P,
00062       typename Math_P
00063    >
00064    int
00065    DirectMotion<OsModel_P, Position_P, ControlledMotion_P, Math_P>::
00066 	init() {
00067       return SUCCESS;
00068    }
00069    
00070    template<
00071       typename OsModel_P,
00072       typename Position_P,
00073       typename ControlledMotion_P,
00074       typename Math_P
00075    >
00076    int
00077    DirectMotion<OsModel_P, Position_P, ControlledMotion_P, Math_P>::
00078 	init(Position_P& position, ControlledMotion_P& cm) {
00079       position_ = &position;
00080       controlled_motion_ = &cm;
00081       return SUCCESS;
00082    }
00083    
00084    template<
00085       typename OsModel_P,
00086       typename Position_P,
00087       typename ControlledMotion_P,
00088       typename Math_P
00089    >
00090    int
00091    DirectMotion<OsModel_P, Position_P, ControlledMotion_P, Math_P>::
00092 	destruct() {
00093       return SUCCESS;
00094    }
00095    
00096    template<
00097       typename OsModel_P,
00098       typename Position_P,
00099       typename ControlledMotion_P,
00100       typename Math_P
00101    >
00102    int
00103    DirectMotion<OsModel_P, Position_P, ControlledMotion_P, Math_P>::
00104 	move_to(const position_t& target) {
00105       position_t current_pos, delta;
00106       typename ControlledMotion::distance_t distance;
00107       typename ControlledMotion::angle_t angle;
00108       
00109       current_pos = (*position_)();
00110       delta = target - current_pos;
00111       
00112       distance = Math::sqrt(delta.x() * delta.x() + delta.y() * delta.y());
00113       angle = Math::asin(-delta.x() / distance);
00114       
00115       // Correct quadrant if necessary since asin() can not possibly
00116       // reconstruct it from only the x-position
00117       if(delta.y() < 0) {
00118          angle = Math::sgn(angle) * Math::PI_2 - angle;
00119       }
00120       
00121       controlled_motion_->turn_to(angle);
00122       controlled_motion_->robot().wait_for_stop();
00123       controlled_motion_->move_distance(distance);
00124       controlled_motion_->robot().wait_for_stop();
00125       
00126       return SUCCESS;
00127    }
00128    
00129 } // namespace
00130 
00131 #endif // DIRECT_MOTION_H
00132