Joint.cpp - OpenExo

read_data()

源代码

void _Joint::read_data()  
{//Read the torque sensor, and change sign based on side._joint_data->torque_reading = (_joint_data->flip_direction ? -1.0 : 1.0) * _torque_sensor.read();_joint_data->position = _joint_data->motor.p / _joint_data->motor.gearing;_joint_data->velocity = _joint_data->motor.v / _joint_data->motor.gearing;//Read the true torque sensor offset_joint_data->torque_offset_reading = _torque_sensor.readOffset();//Return calculated torque reading based on the offset pulled from the SD Card_joint_data->torque_reading_microSD = (_joint_data->flip_direction ? -1.0 : 1.0) * _torque_sensor.read_microSD(_joint_data->torque_offset / 100);//To bypass the torque calibration process at the beginning of each trial, modify the torque offset placeholder "255" on the SD card//Example: If the true offset is 1.19, use 119; if the true offset is 0.95, use 95.if (_joint_data->torque_offset != 255){_joint_data->torque_reading = _joint_data->torque_reading_microSD;}
};

功能定位

_Joint::read_data() 是关节（Joint）对象的传感器数据采集与处理的主函数。它主要负责采集当前关节的力矩、位置、速度等物理量，并把它们存进 JointData 数据结构，为后续控制算法与状态监控提供原始信息。

步骤详解

力矩传感器（Torque Sensor）读取

_joint_data->torque_reading = (_joint_data->flip_direction ? -1.0 : 1.0) * _torque_sensor.read();

• 调用 _torque_sensor.read() 采集当前力矩传感器的原始值（模拟量或数字量）。

• 如果该关节需要方向翻转（如左右腿关节装配反了），则乘以 -1。

• 最终的力矩读数存入 _joint_data->torque_reading。

位置和速度的计算

_joint_data->position = _joint_data->motor.p / _joint_data->motor.gearing;
_joint_data->velocity = _joint_data->motor.v / _joint_data->motor.gearing;

• 电机的数据（位置 p、速度 v）来源于 _joint_data->motor，通常是电机编码器或驱动板返回的数值。

• 电机本体输出轴和实际关节之间存在减速齿轮，所以除以齿轮比 gearing，得到实际关节的角度和速度。

读取力矩传感器的 offset

_joint_data->torque_offset_reading = _torque_sensor.readOffset();

• 调用 readOffset() 得到当前传感器的 offset 值（零漂），通常用于力矩零点校准或温漂补偿。

SD卡校准力矩读数

_joint_data->torque_reading_microSD = (_joint_data->flip_direction ? -1.0 : 1.0) * _torque_sensor.read_microSD(_joint_data->torque_offset / 100);

• 某些情况下（例如批量标定后），校准参数会存在 SD 卡里。这里通过 read_microSD() 读取 SD 卡中存储的 offset 并补偿到当前力矩读数。

• 乘以 -1 做方向翻转，和前面类似。

• 这里的 (_joint_data->torque_offset / 100) 是将存储在 SD 卡的 offset（整数格式，比如119代表1.19）转换为实际值。

跳过实时校准（用SD卡参数直接替换）

if (_joint_data->torque_offset != 255)
{_joint_data->torque_reading = _joint_data->torque_reading_microSD;
}

• SD 卡文件里 offset 为 255 通常代表“未设置”或“用默认流程”。

• 如果不是 255，直接使用 SD 卡的力矩读数，跳过实时校准流程。
  这样可以加快系统启动，无需每次都人工校准。

check_calibration()

源代码

void _Joint::check_calibration()  
{// logger::print("id: ");// logger::print(uint8_t(_id));// logger::print("\t");//Check if we are doing the calibration on the torque sensor_joint_data->calibrate_torque_sensor = _torque_sensor.calibrate(_joint_data->calibrate_torque_sensor);if(_joint_data->calibrate_torque_sensor){_data->set_status(status_defs::messages::torque_calibration);}//logger::print("_Joint::check_calibration\n"); if (_joint_data->motor.do_zero){_motor->zero();}
};

功能定位

_Joint::check_calibration() 主要负责 关节传感器与电机的自检和校准逻辑，确保各个传感器（如力矩传感器TorqueSensor）或电机在试验/上电初期能够被正确校准和归零，为之后的数据采集和控制提供可靠的基线。

步骤详解

力矩传感器校准

_joint_data->calibrate_torque_sensor = _torque_sensor.calibrate(_joint_data->calibrate_torque_sensor);

• 通过调用 TorqueSensor 的 calibrate() 方法，执行校准流程。

• 传入 _joint_data->calibrate_torque_sensor 作为状态/开关，控制是否进行校准。

• calibrate() 可能会返回是否校准完成的标志位（典型的方式是，校准过程需要一段时间/多步触发）。

• 将校准结果写回 _joint_data->calibrate_torque_sensor，使后续代码和主循环能知道校准状态。

校准状态同步到全局

if(_joint_data->calibrate_torque_sensor)
{_data->set_status(status_defs::messages::torque_calibration);
}

• 如果此时力矩传感器还在校准状态，则全局 ExoData 状态设为 torque_calibration。

• 这样可以让主控板/上层逻辑或其他 MCU/PC 知道当前整个外骨骼还在做校准流程，不宜开始正式动作。

电机归零（零点校准）

if (_joint_data->motor.do_zero)
{_motor->zero();
}

• 如果该关节对应的电机需要归零（比如刚上电/需要重新找零），就调用 _motor->zero() 方法让电机进行零点校准。

• 这个通常涉及编码器清零、归位动作或者特殊的自检步。

get_torque_sensor_pin(config_defs::joint_id id, ExoData* exo_data)

负责给每个关节分配力矩传感器的物理引脚，确保多个关节不会抢占同一个硬件IO口，并且只给“已启用/需要力矩传感器”的关节分配对应的引脚。

整体流程

• 先根据传入的关节ID（id）判断关节类型（髋/膝/踝/肘）。

• 判断是左侧（left）还是右侧（right）。

• 只有已启用该关节，且配置要求用力矩传感器（torque_flag == 1） 时，才会分配引脚。

• 每分配一个引脚，就把静态的 used_count 计数器加1，保证不会重复分配同一个引脚。

• 引脚不够或者关节未用时，返回一个“not connected”占位值。

步骤详解

以髋关节为例：

case (uint8_t)config_defs::joint_id::hip:
{if (utils::get_is_left(id) && exo_data->left_side.hip.is_used && exo_data->hip_torque_flag == 1){if (_Joint::left_torque_sensor_used_count < logic_micro_pins::num_available_joints){return logic_micro_pins::torque_sensor_left[_Joint::left_torque_sensor_used_count++];}else{return logic_micro_pins::not_connected_pin;}}else if (!(utils::get_is_left(id)) && exo_data->right_side.hip.is_used && exo_data->hip_torque_flag == 1){if (_Joint::right_torque_sensor_used_count < logic_micro_pins::num_available_joints){return logic_micro_pins::torque_sensor_right[_Joint::right_torque_sensor_used_count++];}else{return logic_micro_pins::not_connected_pin;}}else{return logic_micro_pins::not_connected_pin;}break;
}

说明：

• utils::get_is_left(id)：判断这个id是不是左侧关节。

• is_used：关节是否在本次装配中启用。

• hip_torque_flag：是否要求本关节用力矩传感器。

• left_torque_sensor_used_count、right_torque_sensor_used_count：分配计数器，用于每次分配后下次用下一个IO口。

• 其他关节类型（膝、踝、肘）同理。

物理引脚分配机制

• logic_micro_pins::torque_sensor_left 和 logic_micro_pins::torque_sensor_right 分别是左/右侧可用的物理引脚数组，每次分配一个。

• 如果同一侧分配用完了（超过 num_available_joints），直接返回 not_connected_pin，表示没有多余IO了。

静态变量作用

• left_torque_sensor_used_count、right_torque_sensor_used_count 都是 static 静态变量。

• 作用：确保整个系统所有左/右关节分配的引脚不会冲突。

• 在所有关节初始化时自动依次分配，避免手工指定易错问题。

默认兜底

• 没有匹配类型、未启用、或分配满了都会返回 not_connected_pin，便于主控板/代码直接跳过不用的引脚。

HipJoint::run_joint

源代码

void HipJoint::run_joint()
{#ifdef JOINT_DEBUGlogger::print("HipJoint::run_joint::Start");#endif//Make sure the correct controller is running.set_controller(_joint_data->controller.controller);//Calculate the motor command_joint_data->controller.setpoint = _controller->calc_motor_cmd();//Check for joint errorsconst uint16_t exo_status = _data->get_status();const bool correct_status = (exo_status == status_defs::messages::trial_on) || (exo_status == status_defs::messages::fsr_calibration) || (exo_status == status_defs::messages::fsr_refinement);const bool error = correct_status ? _error_manager.run(_joint_data) : false;if (error) {//Send all errors to the other microcontrollerfor (int i=0; i < _error_manager.errorQueueSize(); i++){_motor->set_error();ErrorReporter::get_instance()->report(_error_manager.popError(),_id);}}//Enable or disable the motor._motor->on_off(); _motor->enable();               //Do not enable the motors until the GUI tells you to, defaults to not enabled to prevent running until desired. //Send the new command to the motor._motor->transaction(_joint_data->controller.setpoint / _joint_data->motor.gearing);#ifdef JOINT_DEBUGlogger::print("HipJoint::run_joint::Motor Command:: ");logger::print(_controller->calc_motor_cmd());logger::print("\n");#endif};  

分步逻辑说明

选择高层控制器

set_controller(_joint_data->controller.controller);

• 动态设置本关节当前使用的高层控制器（如零力矩、常值力矩、特定算法等）。

• 支持在运行过程中实时切换控制策略，比如实验不同控制器、失效时切回安全控制器等。

计算新的目标输出

_joint_data->controller.setpoint = _controller->calc_motor_cmd();

• 调用已设置的高层控制器的 calc_motor_cmd() 方法，计算本关节本次控制周期的目标输出（如期望力矩/位置/速度）。

• 结果写回到 joint data 里，供下游使用。

检查关节错误（安全相关）

const uint16_t exo_status = _data->get_status();
const bool correct_status = (exo_status == ...);
const bool error = correct_status ? _error_manager.run(_joint_data) : false;
if (error) {for (...) {_motor->set_error();ErrorReporter::get_instance()->report(...);}
}

• 状态筛选：只有在“实验运行中、FSR校准、FSR细化”这几种状态下才会真正检查错误。

• ErrorManager 运行，对 joint data 做安全检查，比如传感器/驱动/超限等问题。

• 如果有错误：

  • 本地关节电机设置错误状态，防止误动作。

  • 所有错误通过 ErrorReporter 汇报给上位机或第二个MCU（比如实时监控/日志）。

电机上电/下电

_motor->on_off();
_motor->enable();

• 根据 enable 状态真正上电或下电，防止未授权动作。

• 只有 GUI 或主控允许后才会执行。

电机事务命令发送

_motor->transaction(_joint_data->controller.setpoint / _joint_data->motor.gearing);

• 实际下发本周期新计算的目标命令（经过齿轮比换算，映射到电机原始物理量，比如期望转速/角度/电流）。

• 该事务通常包括CAN总线或PWM等底层通讯。

调试信息

#ifdef JOINT_DEBUGlogger::print("HipJoint::run_joint::Motor Command:: ");logger::print(_controller->calc_motor_cmd());logger::print("\n");
#endif

如果开启调试，会在串口输出本周期控制命令，用于实验/调参/故障分析。

小结（流程总览）

• 动态切换控制器 →

• 计算控制输出 →

• 安全检测与报告 →

• （条件）电机上电 →

• 发出新控制命令

这是典型的“闭环实时控制”的一帧主循环，既有灵活的高层策略，也有安全防护和底层硬件动作。