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