Skip to content

Commit

Permalink
temp
Browse files Browse the repository at this point in the history
  • Loading branch information
Moju Zhao committed Jan 13, 2025
1 parent 1f35c8e commit 316ac2d
Show file tree
Hide file tree
Showing 5 changed files with 233 additions and 110 deletions.
2 changes: 2 additions & 0 deletions aerial_robot_nerve/spinal/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,9 @@ add_message_files(
ServoState.msg
ServoStates.msg
ServoExtendedState.msg
ServoExtendedStates.msg
ServoExtendedCmd.msg
ServoExtendedCmds.msg
ServoControlCmd.msg
ServoTorqueCmd.msg
ServoTorqueStates.msg
Expand Down
231 changes: 157 additions & 74 deletions aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/DJI_M2006/servo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ using namespace DJI_M2006;

float clamp(float val, float limit) { return std::min(std::max(val, -limit), limit); }

Servo::Servo(): servo_state_pub_("servo/extended_states", &servo_state_msg_), servo_cmd_sub_("servo/extended_cmd", &Servo::servoControlCallback, this)
Servo::Servo()
{
last_connected_time_ = 0;
servo_last_pub_time_ = 0;
Expand Down Expand Up @@ -46,36 +46,37 @@ Servo::Servo(): servo_state_pub_("servo/extended_states", &servo_state_msg_), se
p_i_term_ = 0;
}

void Servo::init(CAN_GeranlHandleTypeDef* hcan, osMailQId* handle, ros::NodeHandle* nh, GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
{
/* CAN */
CANDeviceManager::init(hcan, GPIOx, GPIO_Pin);
CANDeviceManager::useRTOS(handle);
CANDeviceManager::addDirectDevice(this);

/* ros */
nh_ = nh;
nh_->advertise(servo_state_pub_);
nh_->subscribe(servo_cmd_sub_);
void Servo::receiveDataCallback(uint32_t identifier, uint32_t dlc, uint8_t* data)
{
int id = identifier - 0x200;

CANDeviceManager::CAN_START();
}
// update map
if (init_cnt_ > 0)
{
std::map<int,Servo>::iterator it = servo_list_.find(id);
if (it == servo_list_.end())
{
servo_list_[id] = Servo();
}
init_cnt_ --;

void Servo::sendData()
{
int16_t goal_m_curr = goal_curr_ * 1000;
uint8_t cmd[8];
cmd[0] = (goal_m_curr >> 8) & 0xFF;
cmd[1] = goal_m_curr & 0xFF;
if (init_cnt_ == 0)
{
servo_states_msg_.servos_length = servo_list_.size();
servo_states_msg_.servos = new spinal::ServoExtendState[servo_list_.size()];
}
}

sendMessage(canTxId, 8, cmd, 0);
uint16_t counts = uint16_t((data[0] << 8) | data[1]);
int16_t rpm = int16_t((data[2] << 8) | data[3]);
int16_t m_curr = int16_t((data[4] << 8) | data[5]);
servo_list_.at(id).update(counts, rpm, m_curr);
}

void Servo::receiveDataCallback(uint32_t identifier, uint32_t dlc, uint8_t* data)
void Servo::update(uint16_t counts, int16_t rpm, int16_t m_curr)
{
uint16_t counts = uint16_t((data[0] << 8) | data[1]);
rpm_ = int16_t((data[2] << 8) | data[3]);
m_curr_ = int16_t((data[4] << 8) | data[5]);
rpm_ = rpm;
m_curr_ = m_curr_;

if (last_pos_measurement_ == 8192) {
last_pos_measurement_ = counts;
Expand Down Expand Up @@ -103,60 +104,27 @@ void Servo::receiveDataCallback(uint32_t identifier, uint32_t dlc, uint8_t* data
filter_vel_p_ -= (filter_vel_p_/GYRO_LPF_FACTOR);
filter_vel_p_ += output_vel_;
filter_vel_ = (filter_vel_p_/GYRO_LPF_FACTOR);

}

void Servo::update(void)
void Servo::control()
{
CANDeviceManager::tick(1);
uint32_t now_time = HAL_GetTick();

/* control */
if( now_time - servo_last_ctrl_time_ >= SERVO_CTRL_INTERVAL)
{
switch (control_mode_)
{
case POS_MODE:
calcPosPid();
break;
case VEL_MODE:
calcVelPid();
break;
case CUR_MODE:
goal_curr_ = goal_value_;
break;
default:
goal_curr_ = 0;
break;
}

goal_curr_ = clamp(goal_curr_, MAX_CURRENT);

sendData();
servo_last_ctrl_time_ = now_time;
}

/* ros publish */

if( now_time - servo_last_pub_time_ >= SERVO_PUB_INTERVAL)
switch (control_mode_)
{
/* send servo */
servo_state_msg_.index = 0;
servo_state_msg_.angle = output_pos_;
servo_state_msg_.velocity = output_vel_;
servo_state_msg_.current = curr_;

servo_state_msg_.current = filter_vel_;

servo_state_pub_.publish(&servo_state_msg_);
servo_last_pub_time_ = now_time;
case POS_MODE:
calcPosPid();
break;
case VEL_MODE:
calcVelPid();
break;
case CUR_MODE:
goal_curr_ = goal_value_;
break;
default:
goal_curr_ = 0;
break;
}
}

void Servo::servoControlCallback(const spinal::ServoExtendedCmd& msg)
{
control_mode_ = msg.mode;
goal_value_ = msg.cmd;
goal_curr_ = clamp(goal_curr_, MAX_CURRENT);
}

void Servo::calcVelPid(void)
Expand All @@ -174,7 +142,6 @@ void Servo::calcVelPid(void)
goal_curr_ = p_term + v_i_term_;
}


void Servo::calcPosPid(void)
{
goal_pos_ = goal_value_;
Expand All @@ -189,3 +156,119 @@ void Servo::calcPosPid(void)

goal_curr_ = p_term + p_i_term_ + d_term;
}

Interface::Interface(): servo_state_pub_("servo/extended_states", &servo_states_msg_),
servo_cmd_sub_("servo/extended_cmds", &Servo::servoControlCallback, this)
{
/* variables */
init_cnt_ = 100; // for catch the CAN messages from servo, 100 messages.
servo_states_msg_.servos_length = 0;
}

void Interface::init(CAN_GeranlHandleTypeDef* hcan, osMailQId* handle, ros::NodeHandle* nh, GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
{
/* CAN */
CANDeviceManager::init(hcan, GPIOx, GPIO_Pin);
CANDeviceManager::useRTOS(handle);
CANDeviceManager::addDirectDevice(this);

/* ros */
nh_ = nh;
nh_->advertise(servo_state_pub_);
nh_->subscribe(servo_cmd_sub_);

CANDeviceManager::CAN_START();
}

void Interface::sendData()
{
bool cmd1_flag = false;
bool cmd2_flag = false;
uint8_t cmd1[8]; // 1~4
uint8_t cmd2[8]; // 5~8

for (std::map<int,Servo>::iterator it = servo_list_.begin(); it != servo_list_.end(); it++)
{
int id = it->first;
float goal_curr = it->second.getGoalCurrent();
int16_t goal_m_curr = goal_curr_ * 1000;

if (id < 5)
{
cmd1_flag = true;

cmd1[(id-1) * 2] = (goal_m_curr >> 8) & 0xFF;
cmd1[(id-1) * 2 + 1] = goal_m_curr & 0xFF;

}
else
{
cmd2_flag = true;

cmd2[(id-5) * 2] = (goal_m_curr >> 8) & 0xFF;
cmd2[(id-5) * 2 + 1] = goal_m_curr & 0xFF;
}
}

if (cmd1_flag) sendMessage(canTxId1, 8, cmd1, 0);
if (cmd2_flag) sendMessage(canTxId2, 8, cmd2, 0);
}

void Interface::udpate(void)
{
CANDeviceManager::tick(1);
uint32_t now_time = HAL_GetTick();

/* control */
if(now_time - servo_last_ctrl_time_ >= SERVO_CTRL_INTERVAL)
{
for (std::map<int,Servo>::iterator it = servo_list_.begin(); it != servo_list_.end(); it++)
{
it->second.control();
}

sendData();
servo_last_ctrl_time_ = now_time;
}

/* ros publish */
if(now_time - servo_last_pub_time_ >= SERVO_PUB_INTERVAL)
{
if (servo_states_msg_.servos_length > 0)
{
servo_states_msg_.stamp = nh_->now();

int i = 0;
for (std::map<int,Servo>::iterator it = servo_list_.begin(); it != servo_list_.end(); it++)
{
spinal::ServoExtendedState servo;

servo.index = it->first;
servo.angle = it->second.getAngle();
servo.velocity = it->second.getVelocity();
servo.current = it->second.getCurrent();

servo_states_msg_.servos[i] = servo;
i++;
}

servo_state_pub_.publish(&servo_states_msg_);
}
servo_last_pub_time_ = now_time;
}
}

void Interface::servoControlCallback(const spinal::ServoExtendedCmds& msg)
{
for (int i = 0; i < msg.servos_length; i++)
{
int id = msg.servos[i].index;
std::map<int,Servo>::iterator it = servo_list_.find(id);

if (it != servo_list_.end())
{
it->setControlMode(mode);
it->setGoalValue(cmd);
}
}
}
Loading

0 comments on commit 316ac2d

Please sign in to comment.