From bfcabad297a40bf956d9fe89f1d5270b84601414 Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Tue, 5 Mar 2024 23:50:49 +0900 Subject: [PATCH] [Spinal][Servo] add interface to set target angle directly --- .../servo/Dynamixel/dynamixel_serial.cpp | 15 ++++++++++ .../servo/Dynamixel/dynamixel_serial.h | 8 ++++- .../mcu_project/lib/Jsk_Lib/servo/servo.cpp | 30 +++++++++++++++++-- .../mcu_project/lib/Jsk_Lib/servo/servo.h | 6 ++++ 4 files changed, 56 insertions(+), 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 index 9532d3923..e10d6b0bf 100644 --- 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 index f2c89522e..23039cc48 100644 --- 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& getServo() {return servo_;} const std::array& 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 index d91ccfcb5..b0b449b61 100644 --- 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& 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 index cf9f62f7a..dd896efb7 100644 --- 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 #include #include +#include #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& servo_map); + + uint32_t rad2Pos(float angle, float scale, uint32_t zero_point_pos){ + return static_cast(zero_point_pos - angle /scale); + } private: /* ROS */