Skip to content

Commit

Permalink
Merge pull request #8 from Li-Jinjie/PR/Spinal/dynamixel
Browse files Browse the repository at this point in the history 
Modify the sequence of reading data
  • Loading branch information
@sugihara-16
sugihara-16 authored Oct 16, 2024
2 parents 70b11db + ec7cfa1 commit c526775
Show file tree
Hide file tree
Showing 4 changed files with 75 additions and 25 deletions.
43 changes: 27 additions & 16 deletions aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/Dynamixel/dynamixel_serial.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -219,6 +219,21 @@ uint8_t DynamixelSerial::getServoIndex(uint8_t id)

void DynamixelSerial::update()
{
/* receive data process */
/* For one round, change from "send -> receive" to " receive -> send" */
/* This setting can accelerate the receiving process */
if(read_status_packet_flag_) {
for (unsigned int i = 0; i < servo_num_; i++) {
if(!servo_[i].send_data_flag_ && !servo_[i].first_get_pos_flag_) continue;
readStatusPacket(instruction_last_.first);
}
if (instruction_last_.first == INST_GET_PRESENT_POS)
{
setROSCommFlag(true);
}
}
read_status_packet_flag_ = false;

uint32_t current_time = HAL_GetTick();

/* send position command to servo */
Expand Down Expand Up @@ -312,7 +327,6 @@ void DynamixelSerial::update()
std::pair<uint8_t, uint8_t> instruction;
while(instruction_buffer_.pop(instruction))
{
bool read_status_packet_flag = false;
uint8_t servo_index = instruction.second;

if (mutex_ != NULL) osMutexWait(*mutex_, osWaitForever);
Expand Down Expand Up @@ -400,49 +414,46 @@ void DynamixelSerial::update()
switch (instruction.first) {
case INST_GET_PRESENT_POS: /* read servo position(angle) */
cmdSyncReadPresentPosition(false);
read_status_packet_flag = true;
read_status_packet_flag_ = true;
break;
case INST_GET_PRESENT_CURRENT: /* read servo load */
cmdSyncReadPresentCurrent(false);
read_status_packet_flag = true;
read_status_packet_flag_ = true;
break;
case INST_GET_PRESENT_TEMPERATURE: /* read servo temp */
cmdSyncReadPresentTemperature(false);
read_status_packet_flag = true;
read_status_packet_flag_ = true;
break;
case INST_GET_PRESENT_MOVING: /* read servo movement */
cmdSyncReadMoving(false);
read_status_packet_flag = true;
read_status_packet_flag_ = true;
break;
case INST_GET_HARDWARE_ERROR_STATUS:
cmdSyncReadHardwareErrorStatus(false);
read_status_packet_flag = true;
read_status_packet_flag_ = true;
break;
case INST_GET_HOMING_OFFSET:
cmdSyncReadHomingOffset(false);
read_status_packet_flag = true;
read_status_packet_flag_ = true;
break;
case INST_GET_POSITION_GAINS:
cmdSyncReadPositionGains(false);
read_status_packet_flag = true;
read_status_packet_flag_ = true;
break;
case INST_GET_PROFILE_VELOCITY:
cmdSyncReadProfileVelocity(false);
read_status_packet_flag = true;
read_status_packet_flag_ = true;
break;
case INST_GET_CURRENT_LIMIT:
cmdSyncReadCurrentLimit(false);
read_status_packet_flag = true;
read_status_packet_flag_ = true;
break;
default:
break;
}
/* receive data process */
if(read_status_packet_flag) {
for (unsigned int i = 0; i < servo_num_; i++) {
if(!servo_[i].send_data_flag_ && !servo_[i].first_get_pos_flag_) continue;
readStatusPacket(instruction.first);
}

if (read_status_packet_flag_) {
instruction_last_ = instruction;
}
}

Expand Down
12 changes: 12 additions & 0 deletions aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/Dynamixel/dynamixel_serial.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -345,6 +345,10 @@ class DynamixelSerial
const std::array<ServoData, MAX_SERVO_NUM>& getServo() const {return servo_;}
ServoData& getOneServo(uint8_t id);
uint8_t getServoIndex(uint8_t id);

void setROSCommFlag(bool flag) {flag_send_ros_ = flag;}
bool getROSCommFlag() const {return flag_send_ros_;}

private:
UART_HandleTypeDef* huart_; // uart handlercmdReadPresentPosition
osMutexId* mutex_; // for UART (RS485) I/O mutex
Expand All @@ -364,6 +368,14 @@ class DynamixelSerial
// uint8_t rx_buf_[RX_BUFFER_SIZE];
uint32_t rd_ptr_;

/* ros comm */
bool flag_send_ros_ = false;

/* a new and quicker method to read servo data */
bool read_status_packet_flag_ = false;
std::pair<uint8_t, uint8_t> instruction_last_ = std::make_pair(255, 255);


void transmitInstructionPacket(uint8_t id, uint16_t len, uint8_t instruction, uint8_t* parameters);
int8_t readStatusPacket(uint8_t status_packet_instruction);

Expand Down
43 changes: 35 additions & 8 deletions aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,13 +23,15 @@ void DirectServo::init(UART_HandleTypeDef* huart, ros::NodeHandle* nh, osMutexI
nh_->advertiseService(board_info_srv_);

//temp
servo_state_msg_.servos_length = MAX_SERVO_NUM;
servo_state_msg_.servos = new spinal::ServoState[MAX_SERVO_NUM];
servo_torque_state_msg_.torque_enable_length = MAX_SERVO_NUM;
servo_torque_state_msg_.torque_enable = new uint8_t[MAX_SERVO_NUM];

servo_handler_.init(huart, mutex);

unsigned int actual_servo_num = servo_handler_.getServoNum();

servo_state_msg_.servos_length = actual_servo_num;
servo_state_msg_.servos = new spinal::ServoState[actual_servo_num];
servo_torque_state_msg_.torque_enable_length = actual_servo_num;
servo_torque_state_msg_.torque_enable = new uint8_t[actual_servo_num];

servo_last_pub_time_ = 0;
servo_torque_last_pub_time_ = 0;

Expand All @@ -42,15 +44,38 @@ void DirectServo::init(UART_HandleTypeDef* huart, ros::NodeHandle* nh, osMutexI
void DirectServo::update()
{
servo_handler_.update();
sendData();

sendData(true);
}

void DirectServo::sendData()
void DirectServo::sendData(bool flag_send_asap)
{
uint32_t now_time = HAL_GetTick();

if (flag_send_asap && servo_handler_.getROSCommFlag() == true) // This setting will ignore the setting of SERVO_PUB_INTERVAL and pub the information once the measurement is updated.
{
servo_state_msg_.stamp = nh_->now();
for (unsigned int i = 0; i < servo_handler_.getServoNum(); i++) {
const ServoData& s = servo_handler_.getServo()[i];
if (s.send_data_flag_ != 0) {
spinal::ServoState servo;
servo.index = i;
servo.angle = s.present_position_;
servo.temp = s.present_temp_;
servo.load = s.present_current_;
servo.error = s.hardware_error_status_;
servo_state_msg_.servos[i] = servo;
}
}
servo_state_pub_.publish(&servo_state_msg_);
servo_last_pub_time_ = now_time;

servo_handler_.setROSCommFlag(false);
}
else
{
if( now_time - servo_last_pub_time_ >= SERVO_PUB_INTERVAL)
{
servo_state_msg_.stamp = nh_->now();
for (unsigned int i = 0; i < servo_handler_.getServoNum(); i++) {
const ServoData& s = servo_handler_.getServo()[i];
if (s.send_data_flag_ != 0) {
Expand All @@ -66,6 +91,8 @@ void DirectServo::sendData()
servo_state_pub_.publish(&servo_state_msg_);
servo_last_pub_time_ = now_time;
}
}

if( now_time - servo_torque_last_pub_time_ >= SERVO_TORQUE_PUB_INTERVAL)
{
for (unsigned int i = 0; i < servo_handler_.getServoNum(); i++) {
Expand Down
2 changes: 1 addition & 1 deletion aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ class DirectServo

void init(UART_HandleTypeDef* huart, ros::NodeHandle* nh, osMutexId* mutex);
void update();
void sendData();
void sendData(bool flag_send_asap);
void torqueEnable(const std::map<uint8_t, float>& servo_map);
void setGoalAngle(const std::map<uint8_t, float>& servo_map, uint8_t value_type = 0);
DynamixelSerial& getServoHnadler() {return servo_handler_;}
Expand Down

0 comments on commit c526775

Please sign in to comment.