cob_3d_mapping_demonstrator: demonstrator_control

Go to the documentation of this file.
00001 /*
00002  * MapDemonCtrl_Maestro.h
00003  *
00004  *  Created on: 16.11.2012
00005  *      Author: josh
00006  */
00007 
00008 #ifndef MAPDEMONCTRL_MAESTRO_H_
00009 #define MAPDEMONCTRL_MAESTRO_H_
00010 
00011 #include "cob_3d_mapping_demonstrator/demonstrator_control.h"
00012 
00013 class MapDemonCtrlMaestro : public MapDemonCtrl
00014 {
00015 public:
00016 
00018   MapDemonCtrlMaestro(DemonstratorParams * params);
00019 
00021   virtual ~MapDemonCtrlMaestro();
00022 
00023   virtual bool init(DemonstratorParams * params);
00024 
00025   virtual bool runCalibration();
00026 
00027   virtual bool movePos( const std::vector<double>& target_positions );
00028   //virtual bool MoveVel( const std::vector<double>& target_velocities );
00029 
00030   virtual bool close() ;
00031   virtual bool stop();
00032   virtual bool recover();
00033 
00034   virtual void setVelocity();
00035   virtual void setAcceleration();
00036 
00037   virtual bool updatePositions();
00038   virtual bool isMoving();
00039 
00040   bool is_moving_;
00041 
00042 private:
00043 
00044   enum {
00045     GET_POSITION=0x90,
00046     SET_TARGET=0x84,
00047     SET_VEL=0x87,
00048     SET_ACCEL=0x89,
00049     IS_MOVING=0x93,
00050     GO_HOME=0xA2
00051   };
00052 
00053   // Handle for serial device
00054   int fd_;
00055 
00056   //const double STEP_WIDTH;
00057 
00058   void writeCmd(const unsigned char cmd, const unsigned char channel=255, const unsigned char *data=NULL, const int size=0);
00059 
00060   int rad2int(const double v, const int dof = 0) {
00061     double rad = v;
00062     //rad+=M_PI;
00063     //if(rad<0) rad = 2*M_PI+rad;
00064     return round((1/M_PI*rad*1800 + 1500)*4);
00065     //return round( 2*4900*rad/(2*M_PI) + 1000);
00066     //return round( (v-m_params_->GetOffsets()[dof])*STEP_WIDTH);
00067   }
00068 
00069   double int2rad(const int v, const int dof = 0) {
00070     return (v/4-1500)*M_PI/1800;
00071     //return (v-1000)*(2*M_PI)/(2*4900) - M_PI;
00072     //return (v/STEP_WIDTH)+m_params_->GetOffsets()[dof];
00073   }
00074 };
00075 
00076 #endif /* MAPDEMONCTRL_MAESTRO_H_ */