CHG: move most definitions to header files

Author: slimasziom

robotiq_hande_driver/include/application.hpp

@@ -9,6 +9,12 @@
 namespace hande_driver
 {
 
+constexpr auto kGripperPositionMin = 0.0;
+constexpr auto kGripperPositionMax = 0.05;
+constexpr auto kGripperPositionStep = (kGripperPositionMax - kGripperPositionMin) / 255.0;
+constexpr auto kGripperCurrentScale = 0.01;
+constexpr auto kMaxSpeed = 255;
+constexpr auto kMaxForce = 255;
 
 /**
  * @brief This class contains high level gripper commands and status
@@ -32,7 +38,7 @@ class ApplicationLayer{
 
     ApplicationLayer();
 
-    ~ApplicationLayer();
+    ~ApplicationLayer() {};
 
     /**
      * @brief Stops movement of the gripper
@@ -41,7 +47,9 @@ class ApplicationLayer{
      * @return none
      * @note The status should be checked to verify successful execution. An exception is thrown if communication issues occur.
      */
-    void stop();
+    void stop() {
+        protocol_logic_.stop();
+    };
 
     /**
      * @brief Resets the gripper: deactivate and activate again
@@ -50,7 +58,9 @@ class ApplicationLayer{
      * @return none
      * @note see status on success, exception thrown if communicatoin issues
      */
-    void reset();
+    void reset() {
+        protocol_logic_.reset();
+    };
 
     /**
      *  Emergency auto-release, gripper fingers are slowly opened, reactivation necessary
@@ -59,7 +69,9 @@ class ApplicationLayer{
      * @return none
      * @note see status on success, exception thrown if communicatoin issues
      */
-    void auto_release();
+    void auto_release() {
+        protocol_logic_.auto_release();
+    };
 
     /**
      * @brief Activates the gripper, after that it can be used
@@ -68,7 +80,9 @@ class ApplicationLayer{
      * @return none
      * @note see status on success, exception thrown if communicatoin issues
      */
-    void activate();
+    void activate() {
+        protocol_logic_.activate();
+    };
 
     /**
      * @brief Opens the gripper
@@ -77,7 +91,9 @@ class ApplicationLayer{
      * @return none
      * @note see status on success, exception thrown if communicatoin issues
      */
-    void open();
+    void open() {
+        set_position(kGripperPositionMax);
+    };
 
     /**
      * @brief Closes the gripper
@@ -86,7 +102,9 @@ class ApplicationLayer{
      * @return none
      * @note see status on success, exception thrown if communicatoin issues
      */
-    void close();
+    void close() {
+        set_position(kGripperPositionMin);
+    };
 
     /**
      * @brief Returns the gripper status
@@ -95,7 +113,9 @@ class ApplicationLayer{
      * @return Gripper status
      * @note see status on success, exception thrown if communicatoin issues
      */
-    Status get_status();
+    Status get_status() {
+        return status_;
+    };
 
     /**
      * @brief Returns the gripper fault status
@@ -104,7 +124,9 @@ class ApplicationLayer{
      * @return Fault Status
      * @note see status on success, exception thrown if communicatoin issues
      */
-    FaultStatus get_fault_status();
+    FaultStatus get_fault_status() {
+        return fault_status_;
+    };
 
     /**
      * @brief Returns the gripper requested position
@@ -113,7 +135,9 @@ class ApplicationLayer{
      * @return Gripper requested position
      * @note see status on success, exception thrown if communicatoin issues
      */
-    double get_requested_position();
+    double get_requested_position() {
+        return  requested_position_;
+    };
 
     /**
      * @brief Returns the gripper position
@@ -122,7 +146,9 @@ class ApplicationLayer{
      * @return Gripper position in [m]
      * @note see status on success, exception thrown if communicatoin issues
      */
-    double get_position();
+    double get_position() {
+        return position_;
+    };
 
     /**
      * @brief Moves the gripper to requested position
@@ -131,7 +157,10 @@ class ApplicationLayer{
      * @return none
      * @note see status on success, exception thrown if communicatoin issues
      */
-    void set_position(double position);
+    void set_position(double position) {
+        protocol_logic_.go_to(
+            (uint8_t)((kGripperPositionMax - position) / kGripperPositionStep), kMaxSpeed, kMaxForce);
+    };
 
     /**
      * @brief Returns the gripper current
@@ -140,12 +169,15 @@ class ApplicationLayer{
      * @return Gripper current in [A] (range: 0-2.55A)
      * @note see status on success, exception thrown if communicatoin issues
      */
-    double get_current();
-
+    double get_current() {
+        return current_;
+    };
 
     void read();
 
-    void write();
+    void write() {
+        protocol_logic_.refresh_registers();
+    };
 
 private:
     /**

robotiq_hande_driver/include/communication.hpp

@@ -2,10 +2,23 @@
 #define COMMUNICATION_HPP_
 
 #include <modbus/modbus.h>
+#include <string.h>
 
 
-namespace hande_driver
+namespace hande_driver 
 {
+
+constexpr auto kDeviceName = "/tmp/ttyUR";
+constexpr auto kBaudrate = 115200;
+constexpr auto kParity = 'N';
+constexpr auto kDataBits = 8;
+constexpr auto kStopBit = 1;
+constexpr auto kDebugModbus = true;
+constexpr uint8_t kSlaveID = 0x09;
+
+constexpr uint16_t kGripperOutputFirstReg = 0x07D0;
+constexpr uint16_t kGripperInputFirstReg = 0x03E8;
+
 constexpr auto kRegisterWordLength = 3;
 enum class OutputBytes : uint8_t {
     RESERVED_1 = 0u,
@@ -35,7 +48,10 @@ class Communication{
 public:
     Communication();
 
-    ~Communication();
+    ~Communication() {
+        disconnect();
+        modbus_free(mb_);
+    };
 
     /**
      * @brief Connect to gripper using modbus rtu and virtual socket
@@ -53,7 +69,9 @@ class Communication{
      * @return none
      * @note see status on success, exception thrown in case of communication issues
      */
-    void disconnect();
+    void disconnect() {
+        modbus_close(mb_);
+    };
 
     /**
      * @brief Read and write input.output registers at once
@@ -62,7 +80,28 @@ class Communication{
      * @return none
      * @note see status on success, exception thrown in case of communication issues
      */
-    void read_write_registers();
+    void read_write_registers() {
+    /**
+     * @brief Read and write modbus registers at once
+     *
+     * @param modbus_t *ctx
+     * @param int write_addr
+     * @param int write_nb
+     * @param uint16_t *src
+     * @param int read_addr
+     * @param int read_nb
+     * @param uint16_t *dest
+     * @return int >0 on success
+     * @note see status on success, exception thrown in case of communication issues
+     */
+    modbus_write_and_read_registers(mb_,
+                                    kGripperInputFirstReg,
+                                    kRegisterWordLength,
+                                    (uint16_t *)output_bytes_,
+                                    kGripperOutputFirstReg,
+                                    kRegisterWordLength,
+                                    (uint16_t *)input_bytes_);
+    };
 
     /**
      * @brief Set output bytes to zeros
@@ -71,7 +110,9 @@ class Communication{
      * @return none
      * @note see status on success, exception thrown in case of communication issues
      */
-    void clear_output_bytes();
+    void clear_output_bytes() {
+        memset(output_bytes_, 0, sizeof(output_bytes_));
+    };
 
     /**
      * @brief Get input byte value
@@ -80,7 +121,9 @@ class Communication{
      * @return requested byte value
      * @note see status on success, exception thrown in case of communication issues
      */
-    uint8_t get_input_byte(InputBytes index);
+    uint8_t get_input_byte(InputBytes index) {
+        return input_bytes_[(uint)index];
+    };
 
     /**
      * @brief Set output byte value
@@ -90,7 +133,9 @@ class Communication{
      * @return none
      * @note see status on success, exception thrown in case of communication issues
      */
-    void set_output_byte(OutputBytes index, uint8_t value);
+    void set_output_byte(OutputBytes index, uint8_t value) {
+        output_bytes_[(uint)index] = value;
+    };
 
     /**
      * @brief Set n-th bit to action request byte
@@ -99,7 +144,10 @@ class Communication{
      * @param value bool value
      * @return none
      */
-    void write_action_bit(uint8_t position_bit, bool value);
+    void write_action_bit(uint8_t position_bit, bool value) {
+        output_bytes_[(uint)OutputBytes::ACTION_REQUEST] = bit_set_to(
+            output_bytes_[(uint)OutputBytes::ACTION_REQUEST], position_bit, value);
+    };
 
     /**
      * @brief Set n-th bit to x value
@@ -109,7 +157,9 @@ class Communication{
      * @param x bool value
      * @return uint number with n-th bit set to x
      */
-    uint bit_set_to(uint number, uint n, bool x);
+    uint bit_set_to(uint number, uint n, bool x) {
+        return (number & ~((uint)1 << n)) | ((uint)x << n);
+    };
 
 private:
     modbus_t *mb_;