ADD: position and current scaling to application layer

Author: slimasziom

robotiq_hande_driver/include/application.hpp

@@ -84,15 +84,6 @@ class ApplicationLayer{
      */
     void Close();
 
-    /**
-     * @brief Updates gripper status
-     * 
-     * @param none
-     * @return none
-     * @note see status on success, exception thrown if communicatoin issues
-     */
-    void UpdateStatus();
-
     /**
      * @brief Returns the gripper status
      * 
@@ -118,34 +109,34 @@ class ApplicationLayer{
      * @return Gripper requested position 
      * @note see status on success, exception thrown if communicatoin issues
      */
-    uint8_t RequestedPosition();
+    double RequestedPosition();
 
     /**
      * @brief Returns the gripper position
      * 
      * @param none
-     * @return Gripper position in 
+     * @return Gripper position in [m]
      * @note see status on success, exception thrown if communicatoin issues
      */
-    uint8_t Position();
+    double Position();
 
     /**
      * @brief Moves the gripper to requested position
      * 
-     * @param position to which the gripper has to move
+     * @param position to which the gripper has to move, in [m]
      * @return none
      * @note see status on success, exception thrown if communicatoin issues
      */
-    void SetPosition(uint8_t position);
+    void SetPosition(double position);
 
     /**
      * @brief Returns the gripper current
      * 
      * @param none
-     * @return Gripper current in mA (range: 0-2550mA) 
+     * @return Gripper current in [A] (range: 0-2.55A) 
      * @note see status on success, exception thrown if communicatoin issues
      */
-    uint16_t Current();
+    double Current();
 
 
     void Read();
@@ -171,17 +162,17 @@ class ApplicationLayer{
     /**
      * Requested position of the gripper, in [m]
      */
-    uint8_t requested_position_;
+    double requested_position_;
 
     /**
      * POsition of the gripper, in [m]
      */
-    uint8_t position_;
+    double position_;
 
     /**
-     * Current flowin through the gripper, in [mA]
+     * Current flowin through the gripper, in [A]
      */
-    uint16_t current_;
+    double current_;
 
 };
 }   // namespace hande_driver

robotiq_hande_driver/src/application.cpp

@@ -5,6 +5,11 @@
 
 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;
+
 ApplicationLayer::ApplicationLayer()
 :   requested_position_(0)
 ,   position_(0)
@@ -53,19 +58,21 @@ ApplicationLayer::FaultStatus ApplicationLayer::GetFaultStatus(){
     return fault_status_;
 }
 
-uint8_t ApplicationLayer::RequestedPosition(){
+double ApplicationLayer::RequestedPosition(){
     return  requested_position_;
 }
 
-uint8_t ApplicationLayer::Position(){
+double ApplicationLayer::Position(){
     return position_;
 }
 
-void ApplicationLayer::SetPosition(uint8_t position){
-    protocol_logic_.GoTo(position, 255, 255);
+void ApplicationLayer::SetPosition(double position){
+    uint8_t raw_position = uint8_t(position / kGripperPositionStep - kGripperPositionMin);
+
+    protocol_logic_.GoTo(raw_position, 255, 255);
 }
 
-uint16_t ApplicationLayer::Current(){
+double ApplicationLayer::Current(){
     return current_;
 }
 
@@ -82,9 +89,9 @@ void ApplicationLayer::Read(){
 
     //fault_status
 
-    requested_position_ = protocol_logic_.GetRegPos();//TODO: add scaling
-    position_ = protocol_logic_.GetPos();   //TODO: add scaling
-    current_ = (uint16_t)protocol_logic_.GetCurrent() * 10;
+    requested_position_ = (double)protocol_logic_.GetRegPos() * kGripperPositionStep + kGripperPositionMin;
+    position_ = (double)protocol_logic_.GetPos() * kGripperPositionStep + kGripperPositionMin;
+    current_ = (double)protocol_logic_.GetCurrent() * kGripperCurrentScale;
 }
 
 void ApplicationLayer::Write(){