qb_device_driver.h

Go to the documentation of this file.

/***
 *  Software License Agreement: BSD 3-Clause License
 *
 *  Copyright (c) 2016-2018, qbrobotics®
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 *  following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice, this list of conditions and the
 *    following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
 *    following disclaimer in the documentation and/or other materials provided with the distribution.
 *
 *  * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote
 *    products derived from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 *  WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
 *  USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef QB_DEVICE_DRIVER_H
#define QB_DEVICE_DRIVER_H

// Standard libraries
#include <array>

// internal libraries
#include <qbmove_communications.h>

namespace qb_device_driver {
class qbDeviceAPI {
 public:
  virtual void activate(comm_settings *file_descriptor, const int &id, bool activate_command) {
    commActivate(file_descriptor, id, static_cast<char>(activate_command));
    ros::Duration(0.001).sleep();
  }

  virtual void close(comm_settings *file_descriptor) {
    closeRS485(file_descriptor);
  }

  virtual int getCurrents(comm_settings *file_descriptor, const int &id, std::vector<short int> &currents) {
    currents.resize(2);
    return commGetCurrents(file_descriptor, id, currents.data());
  }

  virtual int getDeviceIds(comm_settings *file_descriptor, std::array<char, 255> &device_ids) {
    return RS485ListDevices(file_descriptor, device_ids.data());
  }

  virtual std::string getInfo(comm_settings *file_descriptor, const int &id) {
    char info[5000];
    commGetInfo(file_descriptor, id, INFO_ALL, info);  // actually only INFO_ALL is supported
    return info;
  }

  virtual void getParameters(comm_settings *file_descriptor, const int &id, std::vector<int> &input_mode, std::vector<int> &control_mode, std::vector<int> &resolutions, std::vector<int> &limits) {
    unsigned char parameter_buffer[5000];
    commGetParamList(file_descriptor, id, 0, NULL, 0, 0, parameter_buffer);
    ros::Duration(0.001).sleep();  // unexpected behaviour with no sleep

    getParameter<uint8_t>(5, parameter_buffer, input_mode);  // hardcoded input mode parameter id
    getParameter<uint8_t>(6, parameter_buffer, control_mode);  // hardcoded control mode parameter id
    getParameter<uint8_t>(7, parameter_buffer, resolutions);  // hardcoded encoder resolutions parameter id
    getParameter<int32_t>(11, parameter_buffer, limits);  // hardcoded position limits parameter id
  }

  virtual int getMeasurements(comm_settings *file_descriptor, const int &id, std::vector<short int> &measurements) {
    measurements.resize(5);
    return commGetCurrAndMeas(file_descriptor, id, measurements.data());
  }

  virtual int getPositions(comm_settings *file_descriptor, const int &id, std::vector<short int> &positions) {
    positions.resize(3);
    return commGetMeasurements(file_descriptor, id, positions.data());
  }

  virtual int getSerialPorts(std::array<char[255], 10> &serial_ports) {
    return RS485listPorts(serial_ports.data());
  }

  virtual bool getStatus(comm_settings *file_descriptor, const int &id, bool &activate_status) {
    char status;
    int result = commGetActivate(file_descriptor, id, &status);
    activate_status = status != 0;
    return result == 0;
  }

  virtual bool getStatus(comm_settings *file_descriptor, const int &id) {
    char status;
    return commGetActivate(file_descriptor, id, &status) == 0;
  }

  virtual void open(comm_settings *file_descriptor, const std::string &serial_port) {
    openRS485(file_descriptor, serial_port.c_str());
  }

  virtual int setCommandsAndWait(comm_settings *file_descriptor, const int &id, std::vector<short int> &commands) {
    ROS_ASSERT(commands.size() == 2);
    return commSetInputsAck(file_descriptor, id, commands.data());
  }

  virtual int setCommandsAsync(comm_settings *file_descriptor, const int &id, std::vector<short int> &commands) {
    ROS_ASSERT(commands.size() == 2);
    commSetInputs(file_descriptor, id, commands.data());
    ros::Duration(0.00001).sleep();  // several calls may introduce serial communication errors without a wait (setCommandsAndWait is a more reliable tool)
    return 0;
  }

  virtual int setPID(comm_settings *file_descriptor, const int &id, std::vector<float> &pid) {
    ROS_ASSERT(pid.size() == 3);
    commGetParamList(file_descriptor, id, 2, pid.data(), sizeof(float), pid.size(), NULL);  // actually set the parameter (despite of its name)
    return 0;
  }

 private:
  template<class T>
  void getParameter(const int &parameter_id, unsigned char *parameter_buffer, std::vector<int> &parameter_vector) {
    parameter_vector.clear();
    int number_of_values = parameter_buffer[(parameter_id-1)*PARAM_BYTE_SLOT + 7];
    int value_size = sizeof(T);
    for (int i=0; i<number_of_values; i++) {
      T parameter_field = 0;
      for (int j=0; j<value_size; j++) {
        parameter_field += parameter_buffer[(parameter_id-1)*PARAM_BYTE_SLOT + 8 + i*value_size + value_size - j - 1] << (8 * j);
      }
      parameter_vector.push_back(parameter_field);
    }
  }

  template<class T>
  void getParameter(const int &parameter_id, unsigned char *parameter_buffer, std::vector<float> &parameter_vector) {
    parameter_vector.clear();
    int number_of_values = parameter_buffer[(parameter_id-1)*PARAM_BYTE_SLOT + 7];
    int value_size = sizeof(T);
    for (int i=0; i<number_of_values; i++) {
      std::vector<uint8_t> parameter_field(value_size, 0);
      for (int j=0; j<value_size; j++) {
        parameter_field.at(j) = parameter_buffer[(parameter_id-1)*PARAM_BYTE_SLOT + 8 + i*value_size + value_size - j - 1];
      }
      parameter_vector.push_back(*(reinterpret_cast<T*>(parameter_field.data())));
    }
  }
};
typedef std::shared_ptr<qbDeviceAPI> qbDeviceAPIPtr;
}  // namespace qb_device_driver

#endif // QB_DEVICE_DRIVER_H