Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Modify the sequence of reading data #8

Merged
merged 4 commits into from
Oct 16, 2024
Merged

Modify the sequence of reading data #8

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
30605b6
[Spinal][Dynamixel] change the Dynamixel reading procedure from send-…
Li-Jinjie Sep 20, 2024
3c7857f
[Spinal][Dynamixel] make the send freq. the same as INST_GET_PRESENT_POS
Li-Jinjie Sep 20, 2024
54ce3fc
[Spinal][Dynamixel] refactor code to support quick reading for Dynami…
Li-Jinjie Sep 20, 2024
ec7cfa1
[Spinal][Dynamixel] fix a bug after cherry-pick
Li-Jinjie Oct 3, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
43 changes: 27 additions & 16 deletions aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/Dynamixel/dynamixel_serial.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading