SchunkProtocolArm.h
Go to the documentation of this file.
00001 /*
00002  * SchunkProtocolArm.h
00003  *
00004  * Schunk protocol specific implementation of RobotArm class.
00005  */
00006 
00007 #ifndef SCHUNKPROTOCOLARM_H_
00008 #define SCHUNKPROTOCOLARM_H_
00009 
00010 #include <MetraLabsBase.h>
00011 #include <base/Errors.h>
00012 #include <hardware/SCHUNKMotionManipulator.h>
00013 
00014 #include "RobotArm.h"
00015 
00016 using namespace MetraLabs::robotic::base;
00017 using namespace MetraLabs::robotic::hardware;
00018 
00019 /* these are used here as the Schunk protocol uses degrees */
00020 #define DEG_TO_RAD(d)   ((d)*M_PI/180.0)
00021 #define RAD_TO_DEG(r)   ((r)*180.0/M_PI)
00022 
00023 
00024 class SchunkProtocolArm : public RobotArm {
00025 public:
00026         SchunkProtocolArm(IDType id_offset = 100) : RobotArm(id_offset) {
00027         }
00028 
00029         virtual void init() {
00030                 ROS_INFO("Searching SCHUNK motion modules. Please wait...");
00031                 Error tErr = manipulator_.initialize();
00032                 if (tErr != OK) {
00033                         ROS_FATAL("Failed to initialise the SCHUNK Manipulator. Code: %s\n", getErrorString(tErr).c_str());
00034                         exit(1);
00035                 }
00036                 modules_count_ = manipulator_.getModules().size();
00037 
00038                 ROS_INFO("Found %d SCHUNK modules", modules_count_);
00039         }
00040 
00041         virtual int emergencyStopJoint(IDType id) {
00042                 ROS_WARN("emergency stopping module %d", id + id_offset_);
00043                 return manipulator_.emergencyStop(id + id_offset_);
00044         }
00045 
00046         virtual int normalStopJoint(IDType id) {
00047                 return manipulator_.stop(id + id_offset_);
00048         }
00049 
00050         virtual int ackJoint(IDType id) {
00051                 return manipulator_.ack(id + id_offset_);
00052         }
00053 
00054         virtual int refJoint(IDType id) {
00055                 return manipulator_.ref(id + id_offset_);
00056         }
00057 
00058 
00059         virtual float getPosition(IDType id) {
00060                 return DEG_TO_RAD(manipulator_.getModules().at(id).status_pos);
00061         }
00062         virtual float getVelocity(IDType id) {
00063                 return DEG_TO_RAD(NAN);
00064         }
00065         virtual float getMaxCurrent(IDType id) {
00066                 return manipulator_.getModules().at(id).max_current;
00067         }
00068 
00069 
00070         virtual int setVelocity(IDType id, float velocity) {
00071                 return manipulator_.setTargetVelocity(id + id_offset_, RAD_TO_DEG(velocity));
00072         }
00073         virtual int setAcceleration(IDType id, float acceleration) {
00074                 return manipulator_.setTargetAcceleration(id + id_offset_, RAD_TO_DEG(acceleration));
00075         }
00076         virtual int setCurrent(IDType id, float current) {
00077                 return manipulator_.setTargetCurrent(id + id_offset_, current);
00078         }
00079 
00080 
00081         virtual int movePosition(IDType id, float position) {
00082                 return manipulator_.movePos(id + id_offset_, RAD_TO_DEG(position));
00083         }
00084         virtual int movePositionDuration(IDType id, float position, uint16_t msecs) {
00085                 return ERR_NOT_SUPPORTED;
00086         }
00087         virtual int moveVelocity(IDType id, float velocity) {
00088                 return manipulator_.moveVel(id + id_offset_, RAD_TO_DEG(velocity));
00089         }
00090 
00091 
00092 
00093         virtual void getModuleStatus(IDType module_id, uint8_t& referenced, uint8_t& moving,
00094                         uint8_t& prog_mode, uint8_t& warning, uint8_t& error, uint8_t& brake,
00095                         uint8_t& move_end, uint8_t& pos_reached, uint8_t& error_code, float& current) {
00096                 const SCHUNKMotionManipulator::ModuleConfig *module_config;
00097                 manipulator_.getModuleConfig(module_id + id_offset_, module_config);
00098 
00099                 brake = module_config->status_flags.flags.brake == 1;
00100                 error = module_config->status_flags.flags.error == 1;
00101                 move_end = module_config->status_flags.flags.move_end == 1;
00102                 moving = module_config->status_flags.flags.moving == 1;
00103                 pos_reached = module_config->status_flags.flags.pos_reached == 1;
00104                 prog_mode = module_config->status_flags.flags.prog_mode == 1;
00105                 referenced = module_config->status_flags.flags.referenced == 1;
00106                 warning = module_config->status_flags.flags.warning == 1;
00107                 error_code = module_config->error_code;
00108                 current = module_config->norm_current;
00109         }
00110 
00111 
00112         const SCHUNKMotionManipulator& getManipulator() const {
00113                 return manipulator_;
00114         }
00115 
00116 protected:
00117         SCHUNKMotionManipulator manipulator_;
00118 
00119 };
00120 
00121 #endif /* SCHUNKPROTOCOLARM_H_ */


metralabs_ros
Author(s): Yianni Gatsoulis and Chris Burbridge and Lorenzo Riano and Felix Kolbe
autogenerated on Mon Oct 6 2014 07:27:58