00001
00002
00003
00004
00005 #ifndef ROBOTICAN_HARDWARE_INTERFACE_MOTOR_H
00006 #define ROBOTICAN_HARDWARE_INTERFACE_MOTOR_H
00007
00008
00009 #include <dynamic_reconfigure/server.h>
00010 #include <std_msgs/Float64.h>
00011 #include <robotican_hardware_interface/Device.h>
00012 #include <robotican_hardware_interface/ros_utils.h>
00013 #include <robotican_hardware_interface/RiCBoardManager.h>
00014 #include <robotican_hardware_interface/RiCBoardConfig.h>
00015 #include <robotican_hardware_interface/RiCBoardPotentiometerConfig.h>
00016
00017 #define MOTOR_EPSILON 0.001
00018
00019 namespace robotican_hardware {
00020
00021
00022 class RiCMotor : public Device {
00023 private:
00024 byte _motorAddress;
00025 byte _eSwitchPin;
00026 byte _eSwitchType;
00027 public:
00028 RiCMotor(byte id, TransportLayer *transportLayer, byte motorAddress, byte eSwitchPin, byte eSwitchType);
00029
00030 virtual void deviceAck(const DeviceAck *ack);
00031
00032 virtual void update(const DeviceMessage *deviceMessage)=0;
00033
00034 virtual void write()=0;
00035
00036 virtual void buildDevice()=0;
00037
00038 protected:
00039 byte getESwitchPin();
00040 byte getESwitchType();
00041 byte getAddress();
00042
00043 };
00044
00045 class OpenLoopMotor : public RiCMotor {
00046 private:
00047 JointInfo_t _jointInfo;
00048 byte _encoderPinA;
00049 byte _encoderPinB;
00050 int8_t _motorDirection;
00051 int8_t _encoderDirection;
00052 uint16_t _LPFHzSpeed;
00053 uint16_t _LPFHzInput;
00054 uint16_t _PPR;
00055 float _maxSetPointSpeed;
00056 float _minSetPointSpeed;
00057 float _LPFAlphaSpeed;
00058 float _LPFAlphaInput;
00059
00060 public:
00061
00062 OpenLoopMotor(byte id, TransportLayer *transportLayer, byte motorAddress, byte eSwitchPin, byte eSwitchType,
00063 float maxSetPointSpeed, float minSetPointSpeed, byte encoderA, byte encoderB, int8_t motorDirection,
00064 int8_t encoderDirection, uint16_t LPFHzSpeed, uint16_t LPFHzInput, float LPFAlphaInput,
00065 float LPFAlphaSpeed, uint16_t PPR);
00066
00067 virtual void update(const DeviceMessage *deviceMessage);
00068
00069 virtual void write();
00070
00071 JointInfo_t* getJointInfo();
00072
00073
00074 virtual void buildDevice();
00075 };
00076
00077 struct CloseMotorParams {
00078 uint16_t LPFHzSpeed;
00079 uint16_t PIDHz;
00080 uint16_t PPR;
00081 uint16_t timeout;
00082 int8_t motorDirection;
00083 int8_t encoderDirection;
00084 float LPFAlphaSpeed;
00085 float KP;
00086 float KI;
00087 float KD;
00088 float KFF;
00089 float maxSetPointSpeed;
00090 float limit;
00091 float stopLimitMax;
00092 float stopLimitMin;
00093 };
00094
00095 struct CloseMotorWithEncoderParam : CloseMotorParams {
00096 byte encoderPinA;
00097 byte encoderPinB;
00098 int16_t baisMin;
00099 int16_t baisMax;
00100 float LPFAlphaInput;
00101 uint16_t LPFHzInput;
00102 float maxSetPointPos;
00103 float minSetPointSpeed;
00104 float minSetPointPos;
00105 };
00106
00107 struct CloseMotorWithPotentiometerParam : CloseMotorParams {
00108 byte pin;
00109 float a;
00110 float b;
00111 float tolerance;
00112 uint16_t LPFHzInput;
00113 float LPFAlphaInput;
00114 float maxSetPointPos;
00115 float minSetPointSpeed;
00116 float minSetPointPos;
00117 float K;
00118 uint16_t toleranceTime;
00119 };
00120
00121 class CloseLoopMotor : public RiCMotor {
00122 private:
00123 float _lastCmd;
00124 CloseMotorType::CloseMotorType _motorType;
00125 CloseMotorMode::CloseMotorMode _mode;
00126 bool checkIfLastCmdChange();
00127 public:
00128 CloseLoopMotor(byte id, TransportLayer *transportLayer, byte motorAddress, byte eSwitchPin, byte eSwitchType,
00129 CloseMotorType::CloseMotorType motorType, CloseMotorMode::CloseMotorMode mode);
00130 JointInfo_t* getJointInfo();
00131
00132 virtual void update(const DeviceMessage *deviceMessage);
00133
00134 virtual void write();
00135
00136 virtual void setParams(uint16_t speedLpfHz, uint16_t inputLpfHz, uint16_t pidHz, float speedLpfAlpha,
00137 float inputLpfAlpha, float KP, float KI, float KD, float KFF, float limit)=0;
00138
00139 virtual void buildDevice() = 0;
00140
00141 protected:
00142 JointInfo_t _jointInfo;
00143 CloseMotorType::CloseMotorType getCloseMotorType();
00144 CloseMotorMode::CloseMotorMode getMode();
00145
00146 };
00147
00148 class CloseLoopMotorWithEncoder : public CloseLoopMotor {
00149 private:
00150 CloseMotorWithEncoderParam _params;
00151 ros::NodeHandle _nodeHandle;
00152 boost::recursive_mutex _mutex;
00153 ros::AsyncSpinner _spinner;
00154 ros::Publisher _statusPub;
00155 ros::Timer _timer;
00156 bool _isSetParam;
00157
00158 dynamic_reconfigure::Server <robotican_hardware_interface::RiCBoardConfig> _server;
00159 dynamic_reconfigure::Server<robotican_hardware_interface::RiCBoardConfig>::CallbackType _callbackType;
00160 void dynamicCallback(robotican_hardware_interface::RiCBoardConfig &config, uint32_t level);
00161 void timerCallback(const ros::TimerEvent& e);
00162 virtual void setParams(uint16_t speedLpfHz, uint16_t inputLpfHz, uint16_t pidHz, float speedLpfAlpha,
00163 float inputLpfAlpha, float KP, float KI, float KD, float KFF, float limit);
00164
00165 public:
00166 CloseLoopMotorWithEncoder(byte id, TransportLayer *transportLayer, byte motorAddress, byte eSwitchPin,
00167 byte eSwitchType, CloseMotorType::CloseMotorType motoryType,
00168 CloseMotorMode::CloseMotorMode mode, CloseMotorWithEncoderParam param,
00169 std::string jointName);
00170 virtual void buildDevice();
00171 virtual void write();
00172
00173 };
00174
00175 class CloseLoopMotorWithPotentiometer : public CloseLoopMotor {
00176 private:
00177 bool _isParamChange;
00178 CloseMotorWithPotentiometerParam _param;
00179 ros::NodeHandle _nodeHandle;
00180 bool _firstTime;
00181 boost::recursive_mutex _mutex;
00182
00183 dynamic_reconfigure::Server <robotican_hardware_interface::RiCBoardPotentiometerConfig> _server;
00184 dynamic_reconfigure::Server<robotican_hardware_interface::RiCBoardPotentiometerConfig>::CallbackType _callbackType;
00185 void dynamicCallback(robotican_hardware_interface::RiCBoardPotentiometerConfig &config, uint32_t level);
00186
00187 public:
00188 CloseLoopMotorWithPotentiometer(byte id, TransportLayer *transportLayer,
00189 byte motorAddress, byte eSwitchPin,
00190 byte eSwitchType,
00191 CloseMotorType::CloseMotorType motorType,
00192 CloseMotorMode::CloseMotorMode mode,
00193 CloseMotorWithPotentiometerParam motorParam,
00194 std::string jointName);
00195
00196 virtual void setParams(uint16_t speedLpfHz, uint16_t inputLpfHz, uint16_t pidHz, float speedLpfAlpha,
00197 float inputLpfAlpha, float KP, float KI, float KD, float KFF, float limit);
00198
00199 virtual void setParams(uint16_t speedLpfHz, uint16_t inputLpfHz, uint16_t pidHz, float speedLpfAlpha, float inputLpfAlpha, float KP,
00200 float KI, float KD, float KFF, float K, float a, float b, float tolerance, uint16_t toleranceTime,
00201 float limit);
00202
00203 virtual void buildDevice();
00204
00205 virtual void update(const DeviceMessage *deviceMessage);
00206
00207 virtual void write();
00208 };
00209
00210 }
00211
00212 #endif //ROBOTICAN_HARDWARE_INTERFACE_MOTOR_H