Skip to content

Commit

Permalink
[Spinal][Servo][WIP] workaround send command to dynamixel
Browse files Browse the repository at this point in the history
  • Loading branch information
@sugihara-16
sugihara-16 committed Mar 4, 2024
1 parent 2521f92 commit 91d1380
Show file tree
Hide file tree
Showing 4 changed files with 78 additions and 34 deletions.
54 changes: 39 additions & 15 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 @@ -3,7 +3,7 @@

namespace
{
#if STM32H7
#ifdef STM32H7
uint8_t rx_buf_[RX_BUFFER_SIZE] __attribute__((section(".GpsRxBufferSection")));
#else
uint8_t rx_buf_[RX_BUFFER_SIZE];
Expand Down Expand Up @@ -46,26 +46,40 @@ void DynamixelSerial::init(UART_HandleTypeDef* huart, osMutexId* mutex)
servo_[i].led_ = true;
}
cmdSyncWriteLed();

HAL_Delay(1000);
for (unsigned int i = 0; i < servo_num_; i++) {
servo_[i].led_ = false;
}
cmdSyncWriteLed();

for (int i = 0; i < MAX_SERVO_NUM; i++) {
FlashMemory::addValue(&(servo_[i].p_gain_), 2);
FlashMemory::addValue(&(servo_[i].i_gain_), 2);
FlashMemory::addValue(&(servo_[i].d_gain_), 2);
FlashMemory::addValue(&(servo_[i].profile_velocity_), 2);
FlashMemory::addValue(&(servo_[i].send_data_flag_), 2);
FlashMemory::addValue(&(servo_[i].external_encoder_flag_), 2);
FlashMemory::addValue(&(servo_[i].joint_resolution_), 2);
FlashMemory::addValue(&(servo_[i].servo_resolution_), 2);
FlashMemory::addValue(&(servo_[i].joint_offset_), 4);
// for (int i = 0; i < MAX_SERVO_NUM; i++) {
// FlashMemory::addValue(&(servo_[i].p_gain_), 2);
// FlashMemory::addValue(&(servo_[i].i_gain_), 2);
// FlashMemory::addValue(&(servo_[i].d_gain_), 2);
// FlashMemory::addValue(&(servo_[i].profile_velocity_), 2);
// FlashMemory::addValue(&(servo_[i].send_data_flag_), 2);
// FlashMemory::addValue(&(servo_[i].external_encoder_flag_), 2);
// FlashMemory::addValue(&(servo_[i].joint_resolution_), 2);
// FlashMemory::addValue(&(servo_[i].servo_resolution_), 2);
// FlashMemory::addValue(&(servo_[i].joint_offset_), 4);
// }
// FlashMemory::addValue(&(ttl_rs485_mixed_), 2);

// FlashMemory::read();

for (int i = 0; i < MAX_SERVO_NUM; i++) {
servo_[i].p_gain_ = 2200;
servo_[i].i_gain_ = 100;
servo_[i].d_gain_ = 3000;
servo_[i].profile_velocity_ = 10;
servo_[i].send_data_flag_ = 1;
servo_[i].external_encoder_flag_ = 0;
servo_[i].joint_resolution_ = 1;
servo_[i].servo_resolution_ = 1;
servo_[i].joint_offset_ = 0;
}
FlashMemory::addValue(&(ttl_rs485_mixed_), 2);

FlashMemory::read();
ttl_rs485_mixed_ = 0;

cmdSyncWritePositionGains();
cmdSyncWriteProfileVelocity();
Expand Down Expand Up @@ -191,6 +205,16 @@ void DynamixelSerial::setCurrentLimit(uint8_t servo_index)
if (mutex_ != NULL) osMutexRelease(*mutex_);
}

ServoData& DynamixelSerial::getOneServo(uint8_t id)
{
for(int i = 0; i < servo_num_; i++)
{
if(servo_[i].id_ == id) return servo_[i];
}
ServoData non_servo;
return non_servo;
}

void DynamixelSerial::update()
{
uint32_t current_time = HAL_GetTick();
Expand Down Expand Up @@ -481,7 +505,7 @@ void DynamixelSerial::transmitInstructionPacket(uint8_t id, uint16_t len, uint8_

/* send data */
// WE;
HAL_UART_Transmit(huart_, transmit_data, transmit_data_index, 10); //timeout: 10 ms. Although we found 2 ms is enough OK for our case by oscilloscope. Large value is better for UART async task in RTOS.
HAL_UART_Transmit(huart_, transmit_data, transmit_data_index, 10); //timeout: 10 ms. Although we found 2 ms is enough OK for our case by oscilloscope. Large value is better for UART async task in RTOS.
// RE;
}

Expand Down
2 changes: 1 addition & 1 deletion 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 @@ -338,7 +338,7 @@ class DynamixelSerial
void setTTLRS485Mixed(uint16_t flag) {ttl_rs485_mixed_ = flag;}
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);
private:
UART_HandleTypeDef* huart_; // uart handlercmdReadPresentPosition
osMutexId* mutex_; // for UART (RS485) I/O mutex
Expand Down
50 changes: 35 additions & 15 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 @@ -8,6 +8,9 @@

#include "servo.h"

#define SERVO_PUB_INTERVAL 20 // 50Hz
#define SERVO_TORQUE_PUB_INTERVAL 1000 // 1Hz

void DirectServo::init(UART_HandleTypeDef* huart, ros::NodeHandle* nh, osMutexId* mutex = NULL) //TODO: support encoder
{
nh_ = nh;
Expand All @@ -16,39 +19,56 @@ void DirectServo::init(UART_HandleTypeDef* huart, ros::NodeHandle* nh, osMutexI
nh_->advertise(servo_state_pub_);
nh_->advertise(servo_torque_state_pub_);

// servo_state_msg_.servos_length = servo_with_send_flag_.size();
// servo_state_msg_.servos = new spinal::ServoState[servo_with_send_flag_.size()];
// servo_torque_state_msg_.torque_enable_length = servo_.size();
// servo_torque_state_msg_.torque_enable = new uint8_t[servo_.size()];
//temp
servo_state_msg_.servos_length = 4;
servo_state_msg_.servos = new spinal::ServoState[4];
servo_torque_state_msg_.torque_enable_length = 4;
servo_torque_state_msg_.torque_enable = new uint8_t[4];

servo_handler_.init(huart, mutex);

servo_last_pub_time_ = 0;
servo_torque_last_pub_time_ = 0;
}

void DirectServo::update()
{
servo_handler_.update();
sendData();

}

void DirectServo::sendData()
{
for (unsigned int i = 0; i < servo_handler_.getServoNum(); i++) {
const ServoData& s = servo_handler_.getServo()[i];
if (s.send_data_flag_ != 0) {
ServoState data(static_cast<int16_t>(s.getPresentPosition()),
s.present_temp_,
s.moving_,
s.present_current_,
s.hardware_error_status_);
// sendMessage(CAN::MESSAGEID_SEND_SERVO_LIST[i], m_slave_id, 8, reinterpret_cast<uint8_t*>(&data), 1);
uint32_t now_time = HAL_GetTick();
if( now_time - servo_last_pub_time_ >= SERVO_PUB_INTERVAL)
{
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 = s.id_;
servo.angle = s.present_position_;
servo.temp = s.present_temp_;
servo.load = s.present_current_;
servo.error = s.goal_position_;
servo_state_msg_.servos[i] = servo;
}
}
servo_state_pub_.publish(&servo_state_msg_);
servo_last_pub_time_ = now_time;
}
}
}

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_.getServo()[control_msg.index[i]];
ServoData& s = servo_handler_.getOneServo(control_msg.index[i]);
if(s == servo_handler_.getOneServo(0)){
nh_->logerror("Invalid Servo ID!");
return;
}
int32_t goal_pos = static_cast<int32_t>(control_msg.angles[i]);
s.setGoalPosition(goal_pos);
if (! s.torque_enable_) {
Expand Down
6 changes: 3 additions & 3 deletions 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 @@ -45,9 +45,9 @@ class DirectServo
spinal::ServoStates servo_state_msg_;
spinal::ServoTorqueStates servo_torque_state_msg_;

const uint8_t SERVO_PUB_INTERVAL = 20; //[ms]
const uint32_t SERVO_TORQUE_PUB_INTERVAL = 1000; //[ms]

uint32_t servo_last_pub_time_;
uint32_t servo_torque_last_pub_time_;
/* Servo state */
struct ServoState{
int16_t angle;
Expand Down

0 comments on commit 91d1380

Please sign in to comment.