00001 /* 00002 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 00003 * 00004 * Licensed under the Apache License, Version 2.0 (the "License"); 00005 * you may not use this file except in compliance with the License. 00006 * You may obtain a copy of the License at 00007 * 00008 * http://www.apache.org/licenses/LICENSE-2.0 00009 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an "AS IS" BASIS, 00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 */ 00016 00017 00018 #ifndef __POWER_CUBE_CTRL_H_ 00019 #define __POWER_CUBE_CTRL_H_ 00020 00021 #define VERSION_ELECTR3_FIRST 0x3518 00022 #define VERSION_ELECTR2_LAST 0x3000 00023 #define VERSION_ELECTR2_FIRST 0x2518 00024 00025 // standard includes 00026 #include <iostream> 00027 #include <sstream> 00028 #include <string> 00029 #include <deque> 00030 #include <pthread.h> 00031 00032 // own includes 00033 #include <schunk_libm5api/m5apiw32.h> 00034 #include <schunk_powercube_chain/moveCommand.h> 00035 #include <schunk_powercube_chain/PowerCubeCtrlParams.h> 00036 00037 class PowerCubeCtrl 00038 { 00039 public: 00041 PowerCubeCtrl(PowerCubeCtrlParams* params); 00042 00044 ~PowerCubeCtrl(); 00045 00046 typedef enum 00047 { 00048 PC_CTRL_OK = 0, 00049 PC_CTRL_NOT_HOMED = -1, 00050 PC_CTRL_ERR = -2, 00051 PC_CTRL_POW_VOLT_ERR = -3 00052 } PC_CTRL_STATUS; 00053 00055 // Functions for initialization and close: // 00057 00061 bool Init(PowerCubeCtrlParams* params); 00062 00066 bool isInitialized() const 00067 { 00068 return m_Initialized; 00069 } 00070 00074 std::string getErrorMessage() const 00075 { 00076 return m_ErrorMessage; 00077 } 00078 00082 PC_CTRL_STATUS getPC_Status() const 00083 { 00084 return m_pc_status; 00085 } 00086 00090 bool Close(); 00091 00093 // Functions for control: // 00095 00099 bool MoveJointSpaceSync(const std::vector<double>& angles); 00100 00104 bool MoveVel(const std::vector<double>& velocities); 00105 00109 bool Stop(); 00110 00114 bool Recover(); 00115 00117 // functions to set parameters: // 00119 00125 bool setMaxVelocity(double velocity); 00126 bool setMaxVelocity(const std::vector<double>& velocities); 00127 00133 bool setMaxAcceleration(double acceleration); 00134 bool setMaxAcceleration(const std::vector<double>& accelerations); 00135 00142 bool setHorizon(double horizon); 00143 00150 double getHorizon(); 00151 00157 bool setSyncMotion(); 00158 00164 bool setASyncMotion(); 00165 00167 // Functions for getting state and monitoring: // 00169 00173 bool updateStates(); 00174 00178 bool getStatus(PC_CTRL_STATUS& status, std::vector<std::string>& errorMessages); 00179 00183 std::vector<unsigned long> getVersion(); 00184 00190 bool statusMoving(); 00191 00195 std::vector<double> getPositions(); 00196 00200 std::vector<double> getVelocities(); 00201 00205 std::vector<double> getAccelerations(); 00206 00210 bool getJointAngles(std::vector<double>& result); 00211 00215 bool getJointVelocities(std::vector<double>& result); 00216 00222 bool doHoming(); 00223 00224 void updateVelocities(std::vector<double> pos_temp, double delta_t); 00225 00226 protected: 00227 pthread_mutex_t m_mutex; 00228 00229 int m_DeviceHandle; 00230 bool m_Initialized; 00231 bool m_CANDeviceOpened; 00232 00233 PowerCubeCtrlParams* m_params; 00234 PC_CTRL_STATUS m_pc_status; 00235 00236 std::vector<unsigned long> m_status; 00237 std::vector<std::string> m_ModuleTypes; 00238 std::vector<unsigned long> m_version; 00239 std::vector<unsigned char> m_dios; 00240 std::vector<double> m_positions; 00241 std::deque< std::vector<double> > m_cached_pos; 00242 std::vector<double> m_velocities; 00243 std::vector<double> m_accelerations; 00244 00245 double m_horizon; 00246 00247 ros::Time m_last_time_pub; 00248 00249 std::string m_ErrorMessage; 00250 00251 bool getPositionAndStatus(int module_id, unsigned long* state, unsigned char* dio, float* position); 00252 }; 00253 00254 #endif