[Spinal][Servo] add interface to set target angle directly

jsk-ros-pkg · Mar 5, 2024 · bfcabad · bfcabad
1 parent a499730
commit bfcabad
Show file tree

Hide file tree

Showing 4 changed files with 56 additions and 3 deletions.
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/Dynamixel/dynamixel_serial.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/Dynamixel/dynamixel_serial.cpp
@@ -93,6 +93,12 @@ void DynamixelSerial::init(UART_HandleTypeDef* huart, osMutexId* mutex)
             if(servo_[i].external_encoder_flag_) encoder_handler_.setOffset(servo_[i].joint_offset_);
           }
 	}
+
+        /*set angle scale values*/
+        for (int i = 0; i < MAX_SERVO_NUM; i++){
+          servo_[i].zero_point_offset_ = 2047;
+          servo_[i].angle_scale_ = 3.1416 / 2047;
+        }
 }
 
 void DynamixelSerial::ping()
@@ -202,6 +208,15 @@ ServoData& DynamixelSerial::getOneServo(uint8_t id)
   return non_servo;
 }
 
+uint8_t DynamixelSerial::getServoIndex(uint8_t id)
+{
+  for(int i = 0; i < servo_num_; i++)
+    {
+      if(servo_[i].id_ == id) return i;
+    }
+  return 0;
+}
+
 void DynamixelSerial::update()
 {
   uint32_t current_time = HAL_GetTick();

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/Dynamixel/dynamixel_serial.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/Dynamixel/dynamixel_serial.h
@@ -306,12 +306,17 @@ class ServoData {
 	bool led_;
 	bool torque_enable_;
 	bool first_get_pos_flag_;
+        float angle_scale_;
+        uint16_t zero_point_offset_;
 
 	void updateHomingOffset() { homing_offset_ = calib_value_ - present_position_;}
 	void setPresentPosition(int32_t present_position) {present_position_ = present_position + internal_offset_;}
 	int32_t getPresentPosition() const {return present_position_;}
 	void setGoalPosition(int32_t goal_position) {goal_position_ = resolution_ratio_ * goal_position - internal_offset_;}
-	int32_t getGoalPosition() const {return goal_position_;}
+        int32_t getGoalPosition() const {return goal_position_;}
+        float getAngleScale() const {return angle_scale_;}
+        uint16_t getZeroPointOffset() const {return zero_point_offset_;}
+
 
 	bool operator==(const ServoData& r) const {return this->id_ == r.id_;}
 };
@@ -339,6 +344,7 @@ class DynamixelSerial
   std::array<ServoData, MAX_SERVO_NUM>& getServo() {return servo_;}
   const std::array<ServoData, MAX_SERVO_NUM>& getServo() const {return servo_;}
   ServoData& getOneServo(uint8_t id);
+  uint8_t getServoIndex(uint8_t id);
 private:
   UART_HandleTypeDef* huart_; // uart handlercmdReadPresentPosition
   osMutexId* mutex_; // for UART (RS485) I/O mutex

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.cpp
@@ -61,11 +61,36 @@ void DirectServo::sendData()
     }
 }
 
+void DirectServo::setGoalAngle(const std::map<uint8_t, float>& servo_map)
+{
+  for (auto servo : servo_map)
+    {
+      uint16_t servo_id = servo.first;
+      float goal_angle = servo.second;
+
+      ServoData& s = servo_handler_.getOneServo(servo_id);
+      uint8_t index = servo_handler_.getServoIndex(servo_id);
+      if(s == servo_handler_.getOneServo(0)){ 
+        nh_->logerror("Invalid Servo ID!");
+        return;
+      }
+
+      uint32_t goal_pos = DirectServo::rad2Pos(goal_angle, s.angle_scale_, s.zero_point_offset_ );
+      s.setGoalPosition(goal_pos);
+      if (! s.torque_enable_) {
+        s.torque_enable_ = true;
+        servo_handler_.setTorque(index);
+      }
+
+    }
+}
+
 void DirectServo::servoControlCallback(const spinal::ServoControlCmd& control_msg)
 {
   if (control_msg.index_length != control_msg.angles_length) return;
   for (unsigned int i = 0; i < control_msg.index_length; i++) {
     ServoData& s = servo_handler_.getOneServo(control_msg.index[i]);
+    uint8_t index = servo_handler_.getServoIndex(control_msg.index[i]);
     if(s == servo_handler_.getOneServo(0)){ 
       nh_->logerror("Invalid Servo ID!");
       return;
@@ -74,7 +99,7 @@ void DirectServo::servoControlCallback(const spinal::ServoControlCmd& control_ms
     s.setGoalPosition(goal_pos);
     if (! s.torque_enable_) {
       s.torque_enable_ = true;
-      servo_handler_.setTorque(i);
+      servo_handler_.setTorque(index);
     }
   }
 }
@@ -84,13 +109,14 @@ void DirectServo::servoTorqueControlCallback(const spinal::ServoTorqueCmd& contr
   if (control_msg.index_length != control_msg.torque_enable_length) return;
   for (unsigned int i = 0; i < control_msg.index_length; i++) {
     ServoData& s = servo_handler_.getOneServo(control_msg.index[i]);
+    uint8_t index = servo_handler_.getServoIndex(control_msg.index[i]);
     if(s == servo_handler_.getOneServo(0)){ 
       nh_->logerror("Invalid Servo ID!");
       return;
     }
     if (! s.torque_enable_) {
       s.torque_enable_ = (control_msg.torque_enable[i] != 0) ? true : false;
-      servo_handler_.setTorque(i);
+      servo_handler_.setTorque(index);
     }
   }
 }

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.h
@@ -18,6 +18,7 @@ includes ------------------------------------------------------------------*/
 #include <spinal/SetDirectServoConfig.h>
 #include <string.h>
 #include <config.h>
+#include <map>
 #include "flashmemory/flashmemory.h"
 
 class Initializer;
@@ -38,6 +39,11 @@ class DirectServo
   void init(UART_HandleTypeDef* huart, ros::NodeHandle* nh, osMutexId* mutex);
   void update();
   void sendData();
+  void setGoalAngle(const std::map<uint8_t, float>& servo_map);
+
+  uint32_t rad2Pos(float angle, float scale, uint32_t zero_point_pos){
+    return static_cast<uint32_t>(zero_point_pos - angle /scale);
+  }
 
 private:
   /* ROS */