project_boditech_vidas_a8000_v3
/
a8000_subboard_sdk
2
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update

master
zhaohe 1 year ago
parent
c28ee14978
commit
193417b741
3 changed files with 139 additions and 462 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 480
      components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
  2. 120
      components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp
  3. 1
      components/step_motor_ctrl_module/step_motor_ctrl_module.cpp

480
components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
View File

@ -1,436 +1,158 @@
#include "mini_servo_motor_ctrl_module.hpp"
#include "a8000_protocol\protocol.hpp"
using namespace iflytop;
using namespace std;
#define TAG "MiniRobotCtrlModule"
#define TAG "MiniServo"
static void limit_input(s32 &input, s32 min, s32 max) {
static int32_t limit_input(int32_t input, int32_t min, int32_t max) {
int32_t ret = 0;
if (input > max) input = max;
if (input < min) input = min;
ret = input;
return ret;
}
void MiniRobotCtrlModule::initialize(uint16_t module_id, FeiTeServoMotor *bus, uint8_t idinbus, flash_config_t *cfg) {
static bool poseq(int32_t a, int32_t b, int32_t err) { return abs(a - b) < err; }
void MiniServoCtrlModule::initialize(uint16_t module_id, FeiTeServoMotor *bus, uint8_t idinbus, config_t *cfg) {
m_bus = bus;
m_id = idinbus;
m_idinbus = idinbus;
m_module_id = module_id;
m_cfg = *cfg;
m_default_cfg = *cfg;
m_cfg = *cfg;
m_bus->write_u8(m_id, feite::kRegServoRunMode, feite::kServoMode);
m_bus->write_u8(m_idinbus, feite::kRegServoRunMode, feite::kServoMode);
m_thread.init("MiniRobotCtrlModule", 1024, osPriorityNormal);
}
void MiniServoCtrlModule::create_default_config(config_t &cfg) {}
enable(1);
int32_t MiniServoCtrlModule::getid(int32_t *id) {
*id = m_module_id;
return 0;
}
int32_t MiniServoCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t &val) {}
int32_t MiniRobotCtrlModule::enable(u8 enable) {
bool suc = m_bus->write_u8(m_id, feite::reg_add_e::kRegServoTorqueSwitch, enable);
int32_t MiniServoCtrlModule::mini_servo_enable(int32_t enable) {
ZLOGI(TAG, "%d mini_servo_enable %d", m_module_id, enable);
bool suc = m_bus->write_u8(m_idinbus, feite::reg_add_e::kRegServoTorqueSwitch, enable);
if (!suc) return err::ksubdevice_overtime;
creg.module_enable = enable;
return err::ksucc;
}
int32_t MiniRobotCtrlModule::stop(u8 stop_type) {
/**
* @brief 切换下当前运行模式再切换回来，可以停止舵机运行
*/
if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
feite::run_mode_e runmode = m_bus->getmode(m_id);
if (runmode == feite::kServoMode) {
m_bus->setmode(m_id, feite::kMotorMode);
}
m_bus->setmode(m_id, feite::kServoMode);
int16_t nowpos = 0;
m_bus->getNowPos(m_id, nowpos);
m_bus->setTargetPos(m_id, nowpos);
m_state.enable = enable;
return err::ksucc;
}
int32_t MiniRobotCtrlModule::position_calibrate(s32 nowpos) {
if (nowpos < 0 || nowpos > 4095) return err::kparam_out_of_range;
if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
if (!m_bus->reCalibration(m_id, nowpos)) return err::ksubdevice_overtime;
int32_t MiniServoCtrlModule::mini_servo_read_pos(int32_t *pos) {
if (!m_bus->getNowPos(m_idinbus, *pos)) return err::ksubdevice_overtime;
return 0;
}
int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, action_cb_status_t status_cb) {
ZLOGI(TAG, "%d rotate speed:%d torque:%d run_time:%d", m_module_id, speed, torque, run_time);
m_thread.stop();
if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
limit_input(torque, 0, 1000);
limit_input(speed, -200000, 200000);
if (!m_bus->rotate(m_id, speed, torque)) return err::ksubdevice_overtime;
int32_t MiniServoCtrlModule::mini_servo_active_cfg() { return 0; }
int32_t MiniServoCtrlModule::mini_servo_stop(int32_t breakstop) { return inter_mini_servo_stop(breakstop); }
m_thread.start([this, speed, torque, run_time, status_cb]() {
int32_t entertime = zos_get_tick();
while (true) {
if (m_thread.getExitFlag() && run_time != 0) break;
if (m_thread.getExitFlag() && run_time == 0) break;
if (run_time != 0 && (zos_haspassedms(entertime) > (uint32_t)run_time)) {
stop(0);
call_status_cb(status_cb, 0);
break;
}
m_thread.sleep(10);
}
ZLOGI(TAG, "%d rotate stop", m_module_id);
});
return 0;
}
int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) {
ZLOGI(TAG, "%d move_to pos:%d speed:%d torque:%d", m_module_id, pos, speed, torque);
int32_t MiniServoCtrlModule::mini_servo_rotate(int32_t direction) {
ZLOGI(TAG, "%d mini_servo_rotate %d", m_module_id, direction);
m_thread.stop();
if (!m_bus->ping(m_idinbus)) return err::ksubdevice_overtime;
if (!m_state.enable) return err::kmini_servo_not_enable;
if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
m_thread.start(
[this, direction]() {
befor_motor_move();
limit_input(torque, 0, 1000);
limit_input(pos, 0, 4095);
{
int32_t velocity = m_cfg.limit_velocity;
if (direction <= 0) velocity = -velocity;
// m_bus->moveTo(m_id, pos, speed, torque);
if (!m_bus->moveTo(m_id, pos, speed, torque)) return err::ksubdevice_overtime;
m_bus->rotate(m_idinbus, direction, m_cfg.limit_torque);
m_thread.start([this, pos, speed, torque, status_cb]() {
while (true) {
uint8_t moveflag = 0;
m_bus->getMoveFlag(m_id, moveflag);
if (moveflag == 0) {
call_status_cb(status_cb, 0);
break;
}
while (true) {
if (m_thread.getExitFlag()) break;
if (!check_when_run()) break;
vTaskDelay(5);
}
}
if (m_thread.getExitFlag()) break;
m_thread.sleep(10);
}
// stop(0);
// ZLOGI(TAG, "%d move_to stop")m_module_id,;
});
after_motor_move();
},
[this]() { inter_mini_servo_stop(false); });
return 0;
}
int32_t MiniRobotCtrlModule::move_forward(s32 velocity, s32 torque) {
ZLOGI(TAG, "%d move_forward torque:%d", m_module_id, torque);
if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
limit_input(torque, 0, 1000);
int32_t MiniServoCtrlModule::mini_servo_move_to(int32_t position) {
ZLOGI(TAG, "%d mini_servo_move_to %d", m_module_id, position);
m_thread.stop();
if (!m_bus->moveTo(m_id, 4095, velocity, torque)) return err::ksubdevice_overtime;
m_thread.start([this, torque]() {
uint8_t moveflag = 0;
while (true) {
if (m_thread.getExitFlag()) break;
m_thread.sleep(10);
if (!m_bus->ping(m_idinbus)) return err::ksubdevice_overtime;
if (!m_state.enable) return err::kmini_servo_not_enable;
m_bus->getMoveFlag(m_id, moveflag);
if (moveflag == 0) {
// 不停止舵机
break;
}
}
});
m_thread.start(
[this, position]() {
befor_motor_move();
return 0;
}
int32_t MiniRobotCtrlModule::move_backward(s32 velocity, s32 torque) {
ZLOGI(TAG, "%d move_backward torque:%d", m_module_id, torque);
{
int32_t velocity = m_cfg.limit_velocity;
int32_t topos = limit_input(position, 1, 4094);
if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
limit_input(torque, 0, 1000);
m_bus->moveTo(m_idinbus, topos, m_cfg.limit_velocity, m_cfg.limit_torque);
m_thread.stop();
if (!m_bus->moveTo(m_id, 0, velocity, torque)) return err::ksubdevice_overtime;
m_thread.start([this, torque]() {
uint8_t moveflag = 0;
while (true) {
if (m_thread.getExitFlag()) break;
m_thread.sleep(10);
uint8_t moveflag = 0;
int32_t nowpos = 0;
m_bus->getMoveFlag(m_id, moveflag);
if (moveflag == 0) {
// 不停止舵机
break;
}
}
});
while (true) {
if (m_thread.getExitFlag()) break;
if (!check_when_run()) break;
return 0;
}
int32_t MiniRobotCtrlModule::move_by(s32 dpos, s32 speed, s32 torque, action_cb_status_t status_cb) {
ZLOGI(TAG, "%d move_by pos:%d speed:%d torque:%d", m_module_id, dpos, speed, torque);
m_thread.stop();
moveflag = 0;
m_bus->getMoveFlag(m_idinbus, moveflag);
m_bus->getNowPos(m_idinbus, nowpos);
if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
limit_input(torque, 0, 1000);
limit_input(speed, -200000, 200000);
if (poseq(nowpos, topos, 10) && moveflag == 0) break;
int16_t nowpos = 0;
m_bus->getNowPos(m_id, nowpos);
vTaskDelay(5);
}
}
int32_t targetpos = nowpos + dpos;
limit_input(targetpos, 0, 4095);
after_motor_move();
},
[this]() {});
move_to(targetpos, speed, torque, [this, nowpos, status_cb](int32_t status) {
if (status_cb) status_cb(status);
});
return 0;
}
int32_t MiniRobotCtrlModule::run_with_torque(s32 torque, s32 run_time, action_cb_status_t status_cb) {
return err::koperation_not_support;
#if 0
ZLOGI(TAG, "%d run_with_torque torque:%d run_time:%d",m_module_id, torque, run_time);
m_thread.stop();
if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
if (torque > 1000) torque = 1000;
if (torque < 0) torque = 0;
if (!m_bus->moveWithTorque(m_id, torque)) return err::ksubdevice_overtime;
int32_t MiniServoCtrlModule::mini_servo_set_mid_point() {
ZLOGI(TAG, "%d mini_servo_set_mid_point", m_module_id);
m_thread.stop();
m_thread.start([this, torque, run_time, status_cb]() {
int32_t entertime = zos_get_tick();
// while (!m_thread.getExitFlag() || zos_haspassedms(entertime) > run_time) {
while (true) {
if (m_thread.getExitFlag() && run_time != 0) {
stop(0);
call_status_cb(status_cb, err::kmodule_opeation_break_by_user);
break;
}
if (m_thread.getExitFlag() && run_time == 0) {
stop(0);
call_status_cb(status_cb, 0);
break;
}
if (run_time != 0 && zos_haspassedms(entertime) > run_time) {
stop(0);
call_status_cb(status_cb, 0);
break;
}
m_thread.sleep(10);
}
stop(0);
if (status_cb) status_cb(0);
});
#endif
bool suc = m_bus->write_u8(m_idinbus, feite::reg_add_e::kRegServoTorqueSwitch, 128);
if (!suc) return err::ksubdevice_overtime;
return 0;
}
int32_t MiniServoCtrlModule::mini_servo_read_io_state(int32_t ioindex) {}
int32_t MiniRobotCtrlModule::move_by_nolimit(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) { return err::koperation_not_support; }
int32_t MiniRobotCtrlModule::read_version(version_t &version) {
uint8_t mainversion, subversion, miniserv_mainversion, miniserv_subversion;
if (!m_bus->readversion(m_id, mainversion, subversion, miniserv_mainversion, miniserv_subversion)) return err::ksubdevice_overtime;
version.version = mainversion << 24 | subversion << 16 | miniserv_mainversion << 8 | miniserv_subversion;
return 0;
void MiniServoCtrlModule::befor_motor_move() {
creg.m_module_status = 1;
creg.module_errorcode = 0;
creg.module_errorbitflag0 = 0;
}
int32_t MiniRobotCtrlModule::read_status(status_t &status) {
if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
FeiTeServoMotor::detailed_status_t feitestatus;
if (!m_bus->read_detailed_status(m_id, &feitestatus)) return err::ksubdevice_overtime;
status.torque = feitestatus.torque;
status.speed = feitestatus.vel;
status.pos = feitestatus.pos;
status.ioState = 0;
if (feitestatus.state != 0) {
status.status = 3;
} else {
status.status = m_thread.isworking();
void MiniServoCtrlModule::after_motor_move() {
if (creg.module_errorcode != 0) {
creg.m_module_status = 2;
}
return 0;
creg.m_module_status = 0;
}
int32_t MiniRobotCtrlModule::read_detailed_status(detailed_status_t &debug_info) {
if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
FeiTeServoMotor::detailed_status_t feitestatus;
if (!m_bus->read_detailed_status(m_id, &feitestatus)) return err::ksubdevice_overtime;
bool MiniServoCtrlModule::check_when_run() { return true; }
if (feitestatus.state != 0) {
debug_info.status = 3;
} else {
debug_info.status = m_thread.isworking();
}
debug_info.ioState = 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;
}
int32_t MiniRobotCtrlModule::set_basic_param(basic_param_t &param) { return err::koperation_not_support; }
int32_t MiniRobotCtrlModule::get_basic_param(basic_param_t &param) {
// ZLOGI(TAG, "%d get_basic_param", m_module_id);
if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
m_bus->read_u16(m_id, feite::kRegServoCalibration, param.posCalibrate);
m_bus->read_u16(m_id, feite::kRegServoMinAngle, param.minlimit);
m_bus->read_u16(m_id, feite::kRegServoMaxAngle, param.maxlimit);
m_bus->read_u8(m_id, feite::kRegServoMaxTemp, param.maxtemp);
m_bus->read_u8(m_id, feite::kRegServoMinVoltage, param.minvoltage);
m_bus->read_u8(m_id, feite::kRegServoMaxVoltage, param.maxvoltage);
m_bus->read_u16(m_id, feite::kRegServoProtectCurrent, param.protectCurrent);
m_bus->read_u8(m_id, feite::kRegServoProtectTorque, param.protectTorque);
return 0;
}
void MiniRobotCtrlModule::call_status_cb(action_cb_status_t cb, int32_t status) {
if (cb) cb(status);
}
/*******************************************************************************
* OVERRIDE MODULE *
*******************************************************************************/
int32_t MiniRobotCtrlModule::module_enable(int32_t venable) { return enable(venable); }
int32_t MiniRobotCtrlModule::getid(int32_t *id) {
*id = m_module_id;
return 0;
}
int32_t MiniRobotCtrlModule::module_stop() {
m_thread.stop();
return stop(0);
}
int32_t MiniRobotCtrlModule::module_break() {
m_thread.stop();
return stop(0);
}
int32_t MiniRobotCtrlModule::module_get_last_exec_status(int32_t *status) {
*status = 0;
return 0;
}
int32_t MiniRobotCtrlModule::module_get_status(int32_t *status) {
*status = m_thread.isworking();
return 0;
}
int32_t MiniRobotCtrlModule::module_get_error(int32_t *iserror) {
/***********************************************************************************************************************
* INTERNAL *
***********************************************************************************************************************/
int32_t MiniServoCtrlModule::inter_mini_servo_stop(int32_t breakstop) {
/**
* @brief TODO:
* 添加过载检测
* @brief 切换下当前运行模式再切换回来，可以停止舵机运行
*/
*iserror = creg.module_errorcode;
return 0;
}
int32_t MiniRobotCtrlModule::module_clear_error() {
creg.module_errorcode = 0;
return 0;
}
int32_t MiniRobotCtrlModule::module_set_reg(int32_t param_id, int32_t param_value) { return module_xxx_reg(param_id, false, param_value); }
int32_t MiniRobotCtrlModule::module_get_reg(int32_t param_id, int32_t *param_value) { return module_xxx_reg(param_id, true, *param_value); }
int32_t MiniRobotCtrlModule::module_readio(int32_t *io) {
*io = 0;
return 0;
}
int32_t MiniRobotCtrlModule::module_read_adc(int32_t adcindex, int32_t *adc) {
*adc = 0;
return 0;
}
int32_t MiniRobotCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t &val) {
switch (param_id) {
MODULE_COMMON_PROCESS_REG_CB();
PROCESS_REG(kreg_step_motor_default_torque, REG_GET(m_cfg.default_torque), REG_SET(m_cfg.default_torque));
PROCESS_REG(kreg_step_motor_default_velocity, REG_GET(m_cfg.default_velocity), REG_SET(m_cfg.default_velocity));
PROCESS_REG(kreg_step_motor_io_state, module_readio(&val), ACTION_NONE);
PROCESS_REG(kreg_step_motor_pos, motor_read_pos(&val), ACTION_NONE);
default:
return err::kmodule_not_find_config_index;
break;
}
return 0;
}
/*******************************************************************************
* Motor *
*******************************************************************************/
int32_t MiniRobotCtrlModule::motor_enable(int32_t varenable) { return enable(varenable); }
int32_t MiniRobotCtrlModule::motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) { return rotate(direction, motor_velocity, acc, nullptr); }
int32_t MiniRobotCtrlModule::motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) { return move_by(distance, motor_velocity, acc, nullptr); }
int32_t MiniRobotCtrlModule::motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) { return move_to(position, motor_velocity, acc, nullptr); }
int32_t MiniRobotCtrlModule::motor_rotate_with_torque(int32_t direction, int32_t torque) {
if (direction >= 0) {
return move_forward(m_cfg.default_velocity, torque);
} else {
return move_backward(m_cfg.default_velocity, torque);
}
}
void MiniRobotCtrlModule::set_errorcode(int32_t errorcode) { creg.module_errorcode = errorcode; }
int32_t MiniRobotCtrlModule::getdpos(int32_t targetpos) {
int32_t nowpos = 0;
m_bus->getNowPos(m_id, nowpos);
int32_t dpos = targetpos - nowpos;
return abs(dpos);
}
int32_t MiniRobotCtrlModule::motor_move_to_torque(int32_t targetpos, int32_t torque, int32_t overtime) {
ZLOGI(TAG, "%d motor_move_to_torque torque:%d", m_module_id, torque);
if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
limit_input(torque, 0, 1000);
m_thread.stop();
if (!m_bus->moveTo(m_id, targetpos, 0, torque)) return err::ksubdevice_overtime;
m_thread.start([this, torque, overtime, targetpos]() {
int32_t entertime = zos_get_tick();
while (true) {
if (m_thread.getExitFlag()) break;
if (overtime != 0 && zos_haspassedms(entertime) > (uint32_t)overtime) {
ZLOGI(TAG, "%d motor_move_to_torque %d overtime", m_module_id, m_id);
stop(0);
set_errorcode(err::kmotor_run_overtime);
break;
}
uint8_t moveFlag = 0;
m_bus->getMoveFlag(m_id, moveFlag);
if (getdpos(targetpos) < 20 && moveFlag == 0) {
ZLOGI(TAG, "%d motor_move_to_torque %d succ", m_module_id, m_id);
stop(0);
break;
}
m_thread.sleep(10);
}
stop(0);
});
return 0;
}
int32_t MiniRobotCtrlModule::motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; }
int32_t MiniRobotCtrlModule::motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; }
int32_t MiniRobotCtrlModule::motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; }
int32_t MiniRobotCtrlModule::motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return err::koperation_not_support; }
int32_t MiniRobotCtrlModule::motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return move_to(0, findzerospeed, acc, nullptr); }
int32_t MiniRobotCtrlModule::motor_read_pos(int32_t *pos) {
if (!m_bus->getNowPos(m_id, *pos)) return err::ksubdevice_overtime;
return 0;
}
int32_t MiniRobotCtrlModule::motor_easy_rotate(int32_t direction) {
if (direction == 1) {
return move_forward(m_cfg.default_velocity, m_cfg.default_torque);
} else if (direction == -1) {
return move_backward(m_cfg.default_velocity, m_cfg.default_torque);
} else {
return err::kparam_out_of_range;
if (!m_bus->ping(m_idinbus)) return err::ksubdevice_overtime;
feite::run_mode_e runmode = m_bus->getmode(m_idinbus);
if (runmode == feite::kServoMode) {
m_bus->setmode(m_idinbus, feite::kMotorMode);
}
}
int32_t MiniRobotCtrlModule::motor_easy_move_by(int32_t distance) { return move_by(distance, m_cfg.default_velocity, m_cfg.default_torque, nullptr); }
int32_t MiniRobotCtrlModule::motor_easy_move_to(int32_t position) { return move_to(position, m_cfg.default_velocity, m_cfg.default_torque, nullptr); }
int32_t MiniRobotCtrlModule::motor_easy_move_to_zero(int32_t direction) { return motor_easy_move_to(0); }
int32_t MiniRobotCtrlModule::motor_easy_set_current_pos(int32_t pos) {
ZLOGI(TAG, "%d motor_easy_set_current_pos %d", m_module_id, pos);
return position_calibrate(pos);
m_bus->setmode(m_idinbus, feite::kServoMode);
int16_t nowpos = 0;
m_bus->getNowPos(m_idinbus, nowpos);
m_bus->setTargetPos(m_idinbus, nowpos);
return err::ksucc;
}

120
components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp
View File

@ -3,105 +3,59 @@
#include "sdk/os/zos.hpp"
//
#include "feite_servo_motor.hpp"
#include "sdk/components/zprotocols/zcancmder/api/i_mini_servo_module.hpp"
#include "a8000_protocol\protocol.hpp"
#include "sdk\components\zcancmder\zcanreceiver.hpp"
namespace iflytop {
class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZIStepMotor {
class MiniServoCtrlModule : public ZIModule, public ZIMiniServo {
ENABLE_MODULE(StepMotorCtrlModule, kmini_servo_motor_module, 0x0001);
public:
typedef struct {
s32 default_torque;
s32 default_velocity;
} flash_config_t;
int32_t limit_velocity;
int32_t limit_torque; // 0->1000
int32_t protective_torque; // 0->1000
} config_t;
private:
FeiTeServoMotor *m_bus;
uint8_t m_id;
uint16_t m_module_id;
ZThread m_thread;
s32 m_pos_shift;
flash_config_t m_cfg = {0};
flash_config_t m_default_cfg = {0};
public:
void initialize(uint16_t module_id, FeiTeServoMotor *bus, uint8_t idinbus, flash_config_t *cfg);
static void create_default_config(flash_config_t &cfg);
virtual int32_t enable(u8 enable);
virtual int32_t stop(u8 stop_type);
virtual int32_t position_calibrate(s32 nowpos);
virtual int32_t rotate(s32 speed, s32 torque, s32 run_time, action_cb_status_t status_cb);
virtual int32_t move_to(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb);
virtual int32_t move_by(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb);
virtual int32_t move_forward(s32 velocity, s32 torque);
virtual int32_t move_backward(s32 velocity, s32 torque);
virtual int32_t run_with_torque(s32 torque, s32 run_time, action_cb_status_t status_cb);
virtual int32_t move_by_nolimit(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb);
virtual int32_t read_version(version_t &version);
virtual int32_t read_status(status_t &status);
virtual int32_t read_detailed_status(detailed_status_t &debug_info);
virtual int32_t set_basic_param(basic_param_t &param);
virtual int32_t get_basic_param(basic_param_t &param);
/*******************************************************************************
* OVERRIDE MODULE *
*******************************************************************************/
virtual int32_t getid(int32_t *id) override;
virtual int32_t module_enable(int32_t enable) override;
virtual int32_t module_stop() override;
virtual int32_t module_break() override;
virtual int32_t module_get_last_exec_status(int32_t *status) override;
virtual int32_t module_get_status(int32_t *status) override;
virtual int32_t module_get_error(int32_t *iserror) override;
virtual int32_t module_clear_error() override;
typedef struct {
int32_t enable;
} state_t;
virtual int32_t module_set_reg(int32_t param_id, int32_t param_value) override;
virtual int32_t module_get_reg(int32_t param_id, int32_t *param_value) override;
private:
uint8_t m_module_id = 0;
virtual int32_t module_readio(int32_t *io) override;
FeiTeServoMotor *m_bus = nullptr;
uint16_t m_idinbus = 0;
virtual int32_t module_read_adc(int32_t adcindex, int32_t *adc) override;
ZThread m_thread;
/*******************************************************************************
* Motor *
*******************************************************************************/
virtual int32_t module_ping() { return 0; };
config_t m_cfg = {0};
state_t m_state = {0};
virtual int32_t motor_enable(int32_t enable) override;
virtual int32_t motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) override;
virtual int32_t motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) override;
virtual int32_t motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) override;
virtual int32_t motor_rotate_with_torque(int32_t direction, int32_t torque) override;
virtual int32_t motor_move_to_torque(int32_t pos, int32_t torque, int32_t overtime) override;
public:
void initialize(uint16_t module_id, FeiTeServoMotor *bus, uint8_t idinbus, config_t *cfg);
static void create_default_config(config_t &cfg);
virtual int32_t motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) override;
virtual int32_t motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) override;
virtual int32_t motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) override;
public:
virtual int32_t getid(int32_t *id);
virtual int32_t module_xxx_reg(int32_t param_id, bool read, int32_t &val);
virtual int32_t motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override;
virtual int32_t motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override;
public:
virtual int32_t mini_servo_enable(int32_t enable);
virtual int32_t mini_servo_read_pos(int32_t *pos);
virtual int32_t mini_servo_active_cfg();
virtual int32_t mini_servo_stop(int32_t breakstop);
virtual int32_t motor_read_pos(int32_t *pos);
virtual int32_t mini_servo_rotate(int32_t direction);
virtual int32_t mini_servo_move_to(int32_t position);
virtual int32_t motor_easy_rotate(int32_t direction) override;
virtual int32_t motor_easy_move_by(int32_t distance) override;
virtual int32_t motor_easy_move_to(int32_t position) override;
virtual int32_t motor_easy_move_to_zero(int32_t direction) override;
virtual int32_t motor_easy_set_current_pos(int32_t pos) override;
virtual int32_t mini_servo_set_mid_point();
virtual int32_t mini_servo_read_io_state(int32_t ioindex);
private:
void call_status_cb(action_cb_status_t cb, int32_t status);
void set_errorcode(int32_t errorcode);
int32_t getdpos(int32_t targetpos);
int32_t module_xxx_reg(int32_t param_id, bool read, int32_t &val);
void befor_motor_move();
void after_motor_move();
bool check_when_run();
int32_t inter_mini_servo_stop(int32_t breakstop);
};
} // namespace iflytop
} // namespace iflytop

1
components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
View File

@ -31,6 +31,7 @@ int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t
switch (param_id) {
MODULE_COMMON_PROCESS_REG_CB();
PROCESS_REG(kreg_step_motor_pos, inter_get_pos(val), ACTION_NONE);
PROCESS_REG(kreg_step_motor_dpos, REG_GET(m_state.dpos), ACTION_NONE);
PROCESS_REG(kreg_step_motor_shaft, REG_GET(m_cfg.motor_shaft), REG_SET(m_cfg.motor_shaft));
PROCESS_REG(kreg_step_motor_one_circle_pulse, REG_GET(m_cfg.motor_one_circle_pulse), REG_SET(m_cfg.motor_one_circle_pulse));
PROCESS_REG(kreg_step_motor_one_circle_pulse_denominator, REG_GET(m_cfg.motor_one_circle_pulse_denominator), REG_SET(m_cfg.motor_one_circle_pulse_denominator));

Write Preview
Loading…
Cancel
Save