ADD: full communication layer to modbus

Author: slimasziom

robotiq_hande_driver/CMakeLists.txt

@@ -6,18 +6,21 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
-find_package(serial REQUIRED)
-
-add_library(
-  include/application.hpp
-  include/protocol_logic.hpp
-  include/communication.hpp
-  src/application.cpp
-  src/protocol_logic.cpp
-  src/communication.cpp
+
+# add_library(
+#   libmodbus
+# )
+
+include_directories(
+  "include"
+  "src"
 )
 
+add_executable(communication src/communication.cpp)
+add_executable(protocol_logic src/protocol_logic.cpp)
+add_executable(application src/application.cpp)
 add_executable(hande_driver src/hande_driver.cpp)
+
 target_include_directories(hande_driver PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)

robotiq_hande_driver/include/application.hpp

@@ -8,23 +8,24 @@
 
 namespace hande_driver
 {
-struct {
-    bool isReset;
-    bool isReady;
-    bool isMoving;
-    bool isStopped;
-    bool isOpened;
-    bool isClosed;
-    bool objectDetected;
-} Status;
-
-struct {             // Structure declaration
-    int myNum;         // Member (int variable)
-    string myString;   // Member (string variable)
-} FaultStatus;
 
 class ApplicationLayer{
-    public:
+public:
+
+    struct Status{
+        bool isReset;
+        bool isReady;
+        bool isMoving;
+        bool isStopped;
+        bool isOpened;
+        bool isClosed;
+        bool objectDetected;
+    };
+
+    struct FaultStatus{
+        bool isError;
+    };
+
     ApplicationLayer();
 
     ~ApplicationLayer();
@@ -90,7 +91,7 @@ class ApplicationLayer{
      * @return none
      * @note see status on success, exception thrown if communicatoin issues
      */
-    void update_status()
+    void update_status();
 
     /**
      * @brief Returns the gripper status
@@ -135,7 +136,7 @@ class ApplicationLayer{
      * @return none
      * @note see status on success, exception thrown if communicatoin issues
      */
-    void ApplicationLayer::set_position(uint8_t position)
+    void set_position(uint8_t position);
 
     /**
      * @brief Returns the gripper current
@@ -146,6 +147,11 @@ class ApplicationLayer{
      */
     uint16_t current();
 
+
+    void read();
+
+    void write();
+
 private:
     /**
      * Protocol logic, used for middle level abstraction

robotiq_hande_driver/include/communication.hpp

@@ -1,14 +1,15 @@
 #include <stdint.h>
 #include <stddef.h>
 #include <vector>
-#include <serial.h>
+#include <modbus/modbus.h>
 
 /**
  * @brief This class contains low level gripper commands and status
  */
 
 namespace hande_driver
 {
+constexpr auto registerWordLength = 3;
 
 class Communication{
 public:
@@ -35,37 +36,19 @@ class Communication{
     void disconnect();
 
     /**
-     * @brief Send command to gripper
+     * @brief Read and write input.output registers at once
      * 
-     * @param data  Data sent to gripper
-     * @param resp_data_len Expected data length of received response
-     * @return none
-     * @note see status on success, exception thrown in case of communication issues
-     */
-    void send_command(const std::vector<uint8_t>& data, size_t resp_data_len);
-
-    /**
-     * @brief Get status frame from gripper
-     * 
-     * @param data_len Data length to read
-     * @return none
-     * @note see status on success, exception thrown in case of communication issues
-     */
-    void get_status(size_t data_len);
-
-    /**
-     * @brief Set callback to be triggered by a timer to update the gripper status
-     * TODO: make a thread?
-     * 
-     * @param freq Refresh rate frequency, no higher than 200Hz including commands
+     * @param none
      * @return none
      * @note see status on success, exception thrown in case of communication issues
      */
-    void arm_get_status_callback(uint16_t freq);
+    void read_write_registers();
 
-protected:
-    serial::Serial serial_;
+    uint16_t input_registers[registerWordLength];
+    uint16_t output_registers[registerWordLength];
 
 private:
+    modbus_t *mb;
+
 };
-}   // namespace hande_driver
+}   // namespace hande_driver

robotiq_hande_driver/include/protocol_logic.hpp

@@ -10,6 +10,7 @@ namespace hande_driver
 {
 
 // Action Request
+constexpr uint8_t actionRequestByte = 0;
 constexpr uint8_t activatePositionByte = 0;             // rACT
 enum Activate{
     DEACTIVATE_GRIPPER,
@@ -35,26 +36,30 @@ enum AutoReleaseDirection{
 };
 
 //Position Request
+constexpr uint8_t positionRequestByte = 3;
 // 0x00 - Open position, with 50 mm opening
 // 0xFF - Closed
 // Opening / count: ≈0.2 mm for 50 mm stroke
 
 //Speed Request
+constexpr uint8_t speedRequestByte = 4;
 // 0x00 - Minimum speed
 // 0xFF - Maximum speed
 
 //Force Request
+constexpr uint8_t forceRequestByte = 5;
 // 0x00 - Minimum force
 // 0xFF - Maximum force
 
 // Gripper Status
+constexpr uint8_t statusByte = 0;
 constexpr uint8_t activationStatusPositionByte = 0;         // gACT
 enum ActivationStatus{
     GRIPPER_RESET,
     GRIPPER_ACTIVATION
 };
 
-constexpr uint8_t goToStatusPositionByte = 3;               // gGTO
+constexpr uint8_t actionStatusPositionByte = 3;               // gGTO
 enum ActionStatus{
     STOPPED,
     GO_TO_POSITION_REQUEST
@@ -77,10 +82,13 @@ enum ObjectDetectionStatus{
 };
 
 // Fault Status
+constexpr uint8_t faultStatusByte = 2;
 // Position Request Echo
+constexpr uint8_t positionRequestEchoByte = 3;
 // Position (current)
+constexpr uint8_t positionByte = 4;
 // Current
-
+constexpr uint8_t currentByte = 5;
 
 class ProtocolLogic{
 public:
@@ -89,14 +97,23 @@ class ProtocolLogic{
     ~ProtocolLogic();
 
     /**
-     *  @brief Resets the gripper: deactivate and activate again
+     *  @brief Resets the gripper
      * 
      * @param none
      * @return none
      * @note see status on success, exception thrown if communicatoin issues
      */
     void reset();
 
+    /**
+     *  @brief Sets the gripper
+     * 
+     * @param none
+     * @return none
+     * @note see status on success, exception thrown if communicatoin issues
+     */
+    void set();
+
     /**
      * @brief Emergency auto-release, gripper fingers are slowly opened, reactivation necessary
      * 
@@ -124,7 +141,7 @@ class ProtocolLogic{
      * @return none
      * @note see status on success, exception thrown if communicatoin issues
      */
-    void go_to(uint8_t position, uint8_t velocity, uint8_t force, bool arm_callback=true);
+    void go_to(uint8_t position, uint8_t velocity, uint8_t force);
 
     /**
      * @brief Stops the gripper
@@ -205,35 +222,48 @@ class ProtocolLogic{
     uint8_t get_current();
 
     /**
-     * @brief Arm a callback to be triggered when gripper starts moving
+     * @brief Decode modbus registers and refresh apropriate data
      * 
      * @return none
      */
-    void wait_until_moving();
+    void refresh_registers();
 
     /**
-     * @brief Arm a callback to be triggered when gripper stops moving
+     * @brief Read 8bit registers from 16bit words
      * 
+     * @param reg pointer to 8bit register
+     * @param byte 8bit register position in 16bit modbus frame
      * @return none
      */
-    void wait_until_stopped();
+    void read_register(uint8_t &reg, uint8_t byte);
+
+    /**
+     * @brief Set n-th bit to x value
+     * 
+     * @param number
+     * @param n n-th bit in number
+     * @param x bool value 
+     * @return uint number with n-th bit set to x
+     */
+    uint bit_set_to(uint number, uint n, bool x);
 
 private:
 // Gripper
+    uint8_t status_;
     ActivationStatus activation_status_;
     ActionStatus action_status_;
     GripperStatus gripper_status_;
     ObjectDetectionStatus object_detection_status_;
 //Fault
-
+    uint8_t fault_status;
 
     /**
-     * @brief Requested position in normalized 0-225 value
+     * @brief Requested position in normalized 0-255 value
      */
-    uint8_t position_request;
+    uint8_t position_request_echo;
 
     /**
-     * @brief Position in normalized 0-225 value
+     * @brief Position in normalized 0-255 value
      */
     uint8_t position;