SVHFingerManager.h

Go to the documentation of this file.

//
// © Copyright 2022 SCHUNK Mobile Greifsysteme GmbH, Lauffen/Neckar Germany
// © Copyright 2022 FZI Forschungszentrum Informatik, Karlsruhe, Germany
//
// This file is part of the Schunk SVH Library.
//
// The Schunk SVH Library is free software: you can redistribute it and/or
// modify it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or (at your
// option) any later version.
//
// The Schunk SVH Library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
// Public License for more details.
//
// You should have received a copy of the GNU General Public License along with
// the Schunk SVH Library. If not, see <https://www.gnu.org/licenses/>.
//

//----------------------------------------------------------------------
//----------------------------------------------------------------------
#ifndef DRIVER_SVH_SVH_FINGER_MANAGER_H_INCLUDED
#define DRIVER_SVH_SVH_FINGER_MANAGER_H_INCLUDED

#include <chrono>
#include <schunk_svh_library/ImportExport.h>
#include <schunk_svh_library/control/SVHController.h>
#include <schunk_svh_library/control/SVHCurrentSettings.h>
#include <schunk_svh_library/control/SVHHomeSettings.h>
#include <schunk_svh_library/control/SVHPositionSettings.h>

#include <memory>
#include <thread>


namespace driver_svh {

class DRIVER_SVH_IMPORT_EXPORT SVHFingerManager
{
public:
  enum Hints
  {
    HT_DEVICE_NOT_FOUND,    /* ttyUSBx could not be found */
    HT_CONNECTION_FAILED,   /* ttyUSB could be opened but communication failed */
    HT_NOT_RESETTED,        /* the fingers of the hand are not resetted */
    HT_NOT_CONNECTED,       /* simply never called connect */
    HT_RESET_FAILED,        /* timeout during reset -> this is a serious failure */
    HT_CHANNEL_SWITCHED_OF, /* Not realy a problem, however a hint worth noting */
    HT_DANGEROUS_CURRENTS,  /* Current Values are set to dangerous levels */
    HT_DIMENSION            /* dummy entry indicating the size, not used as status */
  };

  struct DiagnosticState
  {
    bool diagnostic_encoder_state;
    bool diagnostic_motor_state;
    double diagnostic_current_maximum;
    double diagnostic_current_minimum;
    double diagnostic_position_maximum;
    double diagnostic_position_minimum;
    double diagnostic_deadlock;
  };

  SVHFingerManager(const std::vector<bool>& disable_mask = std::vector<bool>(9, false),
                   const uint32_t& reset_timeout         = 5);

  virtual ~SVHFingerManager();

  bool connect(const std::string& dev_name = "/dev/ttyUSB0", const unsigned int& retry_count = 3);

  void disconnect();

  bool isConnected() { return m_connected; }

  bool resetChannel(const SVHChannel& channel);

  bool enableChannel(const SVHChannel& channel);

  void disableChannel(const SVHChannel& channel);

  bool requestControllerFeedbackAllChannels();

  bool requestControllerFeedback(const SVHChannel& channel);

  bool getPosition(const SVHChannel& channel, double& position);

  bool getCurrent(const SVHChannel& channel, double& current);


  bool setAllTargetPositions(const std::vector<double>& positions);

  bool setTargetPosition(const SVHChannel& channel, double position, double current);

  bool isEnabled(const SVHChannel& channel);

  bool isHomed(const SVHChannel& channel);

  void requestControllerState();

  bool getCurrentSettings(const SVHChannel& channel, SVHCurrentSettings& current_settings);

  bool getPositionSettings(const SVHChannel& channel, SVHPositionSettings& position_settings);

  bool getHomeSettings(const SVHChannel& channel, SVHHomeSettings& home_settings);

  bool getDiagnosticStatus(const SVHChannel& channel, struct DiagnosticState& diagnostic_status);

  bool setCurrentSettings(const SVHChannel& channel, const SVHCurrentSettings& current_settings);

  bool setPositionSettings(const SVHChannel& channel, const SVHPositionSettings& position_settings);

  bool setHomeSettings(const SVHChannel& channel, const SVHHomeSettings& home_settings);

  bool resetDiagnosticData(const SVHChannel& channel);


  // These 3 functions could be private but where made public for printing and debug purposes. As
  // there is no harm to it it should not be a problem

  std::vector<SVHCurrentSettings> getDefaultCurrentSettings();

  std::vector<SVHPositionSettings> getDefaultPositionSettings(const bool& reset = false);

  void setDefaultHomeSettings();

  void setResetSpeed(const float& speed);

  void setResetTimeout(const int& reset_timeout);

  bool setMaxForce(float max_force);

  float setForceLimit(const SVHChannel& channel, float force_limit);

  double convertmAtoN(const SVHChannel& channel, const int16_t& current);

  SVHFirmwareInfo getFirmwareInfo(const std::string& dev_name     = "/dev/ttyUSB0",
                                  const unsigned int& retry_count = 3);


  // ----------------------------------------------------------------------
  // ---- private functions and varaibles
  // ----------------------------------------------------------------------

private:
  SVHController* m_controller;

  std::atomic<bool> m_poll_feedback;

  std::thread m_feedback_thread;

  bool m_connected;

  bool m_connection_feedback_given;

  std::chrono::seconds m_homing_timeout;

  float m_max_current_percentage;

  std::vector<double> m_ticks2rad;

  std::vector<int32_t> m_position_min;

  std::vector<int32_t> m_position_max;

  std::vector<int32_t> m_position_home;

  std::vector<bool> m_is_homed;

  std::vector<bool> m_is_switched_off;

  std::vector<bool> m_diagnostic_encoder_state;

  std::vector<bool> m_diagnostic_current_state;

  std::vector<double> m_diagnostic_current_maximum;

  std::vector<double> m_diagnostic_current_minimum;

  std::vector<double> m_diagnostic_position_maximum;

  std::vector<double> m_diagnostic_position_minimum;

  std::vector<double> m_diagnostic_deadlock;

  float m_reset_speed_factor;

  std::chrono::seconds m_reset_timeout;

  std::vector<SVHCurrentSettings> m_current_settings;

  std::vector<bool> m_current_settings_given;

  std::vector<SVHPositionSettings> m_position_settings;
  std::vector<bool> m_position_settings_given;

  std::vector<SVHHomeSettings> m_home_settings;

  SVHFirmwareInfo m_firmware_info;

  std::string m_serial_device;

  std::vector<SVHChannel> m_reset_order;

  std::vector<double> m_reset_current_factor;

  int32_t convertRad2Ticks(const SVHChannel& channel, const double& position);

  double convertTicks2Rad(const SVHChannel& channel, const int32_t& ticks);

  uint16_t convertNtomA(const SVHChannel& channel, const double& effort);

  bool isInsideBounds(const SVHChannel& channel, const int32_t& target_position);

  bool currentSettingsAreSafe(const SVHChannel& channel,
                              const SVHCurrentSettings& current_settings);

  void pollFeedback();

  // DEBUG
  SVHControllerFeedback m_debug_feedback;
};

} // namespace driver_svh

#endif