00001 // 00002 // Created by tom on 10/05/16. 00003 // 00004 00005 #ifndef ROBOTICAN_HARDWARE_INTERFACE_SERVO_H 00006 #define ROBOTICAN_HARDWARE_INTERFACE_SERVO_H 00007 00008 #include <ros/ros.h> 00009 #include <dynamic_reconfigure/server.h> 00010 #include <robotican_hardware_interface/Device.h> 00011 #include <robotican_hardware_interface/Protocol.h> 00012 #include <robotican_hardware_interface/TransportLayer.h> 00013 #include <robotican_hardware_interface/RiCBoardServoConfig.h> 00014 00015 #define SERVO_EPSILON 0.001 00016 00017 namespace robotican_hardware { 00018 class Servo : public Device { 00019 private: 00020 ros::NodeHandle _nodeHandle; 00021 byte _pin; 00022 float _a; 00023 float _b; 00024 float _max; 00025 float _min; 00026 JointInfo_t _jointInfo; 00027 float _lastCmd; 00028 boost::recursive_mutex _mutex; 00029 dynamic_reconfigure::Server <robotican_hardware_interface::RiCBoardServoConfig> _server; 00030 dynamic_reconfigure::Server<robotican_hardware_interface::RiCBoardServoConfig>::CallbackType _callbackType; 00031 00032 void dynamicCallback(robotican_hardware_interface::RiCBoardServoConfig &config, uint32_t level); 00033 00034 bool _isChangeParam; 00035 00036 bool checkIfLastCmdChange(); 00037 00038 public: 00039 Servo(byte id, TransportLayer *transportLayer, byte pin, float a, float b, float max, float min, 00040 float initPos, std::string jointName); 00041 virtual void update(const DeviceMessage *deviceMessage); 00042 virtual void write(); 00043 virtual void deviceAck(const DeviceAck *ack); 00044 JointInfo_t* getJointInfo(); 00045 void setParam(float a, float b, float max, float min); 00046 virtual void buildDevice(); 00047 00048 00049 }; 00050 00051 } 00052 00053 00054 00055 #endif //ROBOTICAN_HARDWARE_INTERFACE_SERVO_H