PowerCubeCtrl.h

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 
 
#ifndef __POWER_CUBE_CTRL_H_
#define __POWER_CUBE_CTRL_H_
 
#define VERSION_ELECTR3_FIRST 0x3518
#define VERSION_ELECTR2_LAST 0x3000
#define VERSION_ELECTR2_FIRST 0x2518
 
// standard includes
#include <iostream>
#include <sstream>
#include <string>
#include <deque>
#include <pthread.h>
 
// own includes
#include <schunk_libm5api/m5apiw32.h>
#include <schunk_powercube_chain/moveCommand.h>
#include <schunk_powercube_chain/PowerCubeCtrlParams.h>
 
class PowerCubeCtrl
{
public:
  PowerCubeCtrl(PowerCubeCtrlParams* params);
 
  ~PowerCubeCtrl();
 
  typedef enum
  {
    PC_CTRL_OK = 0,
    PC_CTRL_NOT_HOMED = -1,
    PC_CTRL_ERR = -2,
    PC_CTRL_POW_VOLT_ERR = -3
  } PC_CTRL_STATUS;
 
  // Functions for initialization and close: //
 
  bool Init(PowerCubeCtrlParams* params);
 
  bool isInitialized() const
  {
    return m_Initialized;
  }
 
  std::string getErrorMessage() const
  {
    return m_ErrorMessage;
  }
 
  PC_CTRL_STATUS getPC_Status() const
  {
    return m_pc_status;
  }
 
  bool Close();
 
  // Functions for control: //
 
  bool MoveJointSpaceSync(const std::vector<double>& angles);
 
  bool MoveVel(const std::vector<double>& velocities);
 
  bool Stop();
 
  bool Recover();
 
  // functions to set parameters: //
 
  bool setMaxVelocity(double velocity);
  bool setMaxVelocity(const std::vector<double>& velocities);
 
  bool setMaxAcceleration(double acceleration);
  bool setMaxAcceleration(const std::vector<double>& accelerations);
 
  bool setHorizon(double horizon);
 
  double getHorizon();
 
  bool setSyncMotion();
 
  bool setASyncMotion();
 
  // Functions for getting state and monitoring: //
 
  bool updateStates();
 
  bool getStatus(PC_CTRL_STATUS& status, std::vector<std::string>& errorMessages);
 
  std::vector<unsigned long> getVersion();
 
  bool statusMoving();
 
  std::vector<double> getPositions();
 
  std::vector<double> getVelocities();
 
  std::vector<double> getAccelerations();
  
  bool getJointAngles(std::vector<double>& result);
 
  bool getJointVelocities(std::vector<double>& result);
 
  bool doHoming();
 
  void updateVelocities(std::vector<double> pos_temp, double delta_t);
 
protected:
  pthread_mutex_t m_mutex;
 
  int m_DeviceHandle;
  bool m_Initialized;
  bool m_CANDeviceOpened;
 
  PowerCubeCtrlParams* m_params;
  PC_CTRL_STATUS m_pc_status;
 
  std::vector<unsigned long> m_status;
  std::vector<std::string> m_ModuleTypes;
  std::vector<unsigned long> m_version;
  std::vector<unsigned char> m_dios;
  std::vector<double> m_positions;
  std::deque< std::vector<double> > m_cached_pos;
  std::vector<double> m_velocities;
  std::vector<double> m_accelerations;
 
  double m_horizon;
 
  ros::Time m_last_time_pub;
 
  std::string m_ErrorMessage;
 
  bool getPositionAndStatus(int module_id, unsigned long* state, unsigned char* dio, float* position);
};
 
#endif