|
|
|
@ -3,6 +3,7 @@ |
|
|
|
#include "sdk\components\errorcode\errorcode.hpp"
|
|
|
|
using namespace iflytop; |
|
|
|
using namespace std; |
|
|
|
#define TAG "MiniRobotCtrlModule"
|
|
|
|
|
|
|
|
void MiniRobotCtrlModule::initialize(FeiTeServoMotor* bus, uint8_t id) { m_bus->write_u8(m_id, feite::kRegServoRunMode, feite::kServoMode); } |
|
|
|
|
|
|
|
@ -31,15 +32,26 @@ int32_t MiniRobotCtrlModule::position_calibrate(s32 nowpos) { |
|
|
|
if (m_bus->reCalibration(m_id, nowpos)) return err::kce_subdevice_overtime; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 run_time, function<void(rotate_cb_status_t& status)> status_cb) { |
|
|
|
/**
|
|
|
|
* @brief |
|
|
|
* 1:切换成电机模式 |
|
|
|
* 2:写入速度 |
|
|
|
* 3:启动线程，线程退后，设置速度为0 |
|
|
|
*/ |
|
|
|
int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, function<void(rotate_cb_status_t& status)> status_cb) { |
|
|
|
ZLOGI(TAG, "rotate speed:%d torque:%d run_time:%d", speed, torque, run_time); |
|
|
|
m_thread.stop(); |
|
|
|
if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime; |
|
|
|
if (torque > 1000) torque = 1000; |
|
|
|
if (torque < 0) torque = 0; |
|
|
|
|
|
|
|
m_bus->rotate(m_id, speed, torque); |
|
|
|
|
|
|
|
m_thread.start([this, speed, torque, run_time, status_cb]() { |
|
|
|
int32_t entertime = zos_get_tick(); |
|
|
|
while (zos_haspassedms(entertime) > run_time) { |
|
|
|
m_thread.sleep(10); |
|
|
|
} |
|
|
|
stop(0); |
|
|
|
rotate_cb_status_t status; |
|
|
|
status.has_run_time = zos_haspassedms(entertime); |
|
|
|
if (status_cb) status_cb(status); |
|
|
|
}); |
|
|
|
|
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, function<void(move_to_cb_status_t& status)> status_cb) { return 0; } |
|
|
|
@ -70,12 +82,24 @@ int32_t MiniRobotCtrlModule::read_status(status_t& status) { |
|
|
|
} |
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t MiniRobotCtrlModule::read_debug_info(detailed_status_t& debug_info) { |
|
|
|
int32_t MiniRobotCtrlModule::read_detailed_status(detailed_status_t& debug_info) { |
|
|
|
if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime; |
|
|
|
FeiTeServoMotor::status_t feitestatus; |
|
|
|
if (!m_bus->read_status(m_id, &feitestatus)) return err::kce_subdevice_overtime; |
|
|
|
|
|
|
|
FeiTeServoMotor::detailed_status_t feitestatus; |
|
|
|
if (!m_bus->read_detailed_status(m_id, &feitestatus)) return err::kce_subdevice_overtime; |
|
|
|
|
|
|
|
if (feitestatus.state != 0) { |
|
|
|
debug_info.status = 3; |
|
|
|
} else { |
|
|
|
debug_info.status = m_thread.isworking(); |
|
|
|
} |
|
|
|
debug_info.io_state = 0; |
|
|
|
debug_info.torque = feitestatus.torque; |
|
|
|
debug_info.speed = feitestatus.vel; |
|
|
|
debug_info.pos = feitestatus.pos; |
|
|
|
debug_info.voltage = feitestatus.voltage; |
|
|
|
debug_info.current = feitestatus.current; |
|
|
|
debug_info.temperature = feitestatus.temperature; |
|
|
|
debug_info.error_flag = feitestatus.state; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
@ -86,4 +110,3 @@ int32_t MiniRobotCtrlModule::set_warning_limit_param(warning_limit_param_t& para |
|
|
|
int32_t MiniRobotCtrlModule::get_run_param(run_param_t& param) { return 0; } |
|
|
|
int32_t MiniRobotCtrlModule::get_basic_param(basic_param_t& param) { return 0; } |
|
|
|
int32_t MiniRobotCtrlModule::get_warning_limit_param(warning_limit_param_t& param) { return 0; } |
|
|
|
uint8_t MiniRobotCtrlModule::get_module_status() { ; } |
