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

add default velocity to servo

master
zhaohe 2 years ago
parent
0d5a3a53c8
commit
8fdc7be0d0
7 changed files with 32 additions and 192 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. 21
      components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
  2. 31
      components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp
  3. 70
      components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.cpp
  4. 22
      components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.hpp
  5. 64
      components/zcancmder_module/zcan_mini_servo_ctrl_module.cpp
  6. 14
      components/zcancmder_module/zcan_mini_servo_ctrl_module.hpp
  7. 2
      components/zprotocols/zcancmder

21
components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
View File

@ -110,14 +110,14 @@ int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, action_cb_s
return 0;
}
int32_t MiniRobotCtrlModule::move_forward(s32 torque) {
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);
m_thread.stop();
if (!m_bus->moveTo(m_id, 4095, 0, torque)) return err::ksubdevice_overtime;
if (!m_bus->moveTo(m_id, 4095, velocity, torque)) return err::ksubdevice_overtime;
m_thread.start([this, torque]() {
uint8_t moveflag = 0;
while (true) {
@ -134,14 +134,14 @@ int32_t MiniRobotCtrlModule::move_forward(s32 torque) {
return 0;
}
int32_t MiniRobotCtrlModule::move_backward(s32 torque) {
int32_t MiniRobotCtrlModule::move_backward(s32 velocity, s32 torque) {
ZLOGI(TAG, "%d move_backward 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, 0, 0, torque)) return err::ksubdevice_overtime;
if (!m_bus->moveTo(m_id, 0, velocity, torque)) return err::ksubdevice_overtime;
m_thread.start([this, torque]() {
uint8_t moveflag = 0;
while (true) {
@ -334,6 +334,7 @@ int32_t MiniRobotCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t
switch (param_id) {
MODULE_COMMON_PROCESS_REG_CB();
PROCESS_REG(kreg_motor_default_torque, REG_GET(m_cfg.default_torque), REG_SET(m_cfg.default_torque));
PROCESS_REG(kreg_motor_default_velocity, REG_GET(m_cfg.default_velocity), REG_SET(m_cfg.default_velocity));
PROCESS_REG(kreg_module_input_state, module_readio(&val), ACTION_NONE);
PROCESS_REG(kreg_robot_pos, motor_read_pos(&val), ACTION_NONE);
default:
@ -353,9 +354,9 @@ int32_t MiniRobotCtrlModule::motor_move_by(int32_t distance, int32_t motor_veloc
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(torque);
return move_forward(m_cfg.default_velocity, torque);
} else {
return move_backward(torque);
return move_backward(m_cfg.default_velocity, torque);
}
}
void MiniRobotCtrlModule::set_errorcode(int32_t errorcode) { m_com_reg.module_errorcode = errorcode; }
@ -420,15 +421,15 @@ int32_t MiniRobotCtrlModule::motor_read_pos(int32_t *pos) {
}
int32_t MiniRobotCtrlModule::motor_easy_rotate(int32_t direction) {
if (direction == 1) {
return move_forward(m_cfg.default_torque);
return move_forward(m_cfg.default_velocity, m_cfg.default_torque);
} else if (direction == -1) {
return move_backward(m_cfg.default_torque);
return move_backward(m_cfg.default_velocity, m_cfg.default_torque);
} else {
return err::kparam_out_of_range;
}
}
int32_t MiniRobotCtrlModule::motor_easy_move_by(int32_t distance) { return move_by(distance, 0, m_cfg.default_torque, nullptr); }
int32_t MiniRobotCtrlModule::motor_easy_move_to(int32_t position) { return move_to(position, 0, m_cfg.default_torque, nullptr); }
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) { return position_calibrate(pos); }

31
components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp
View File

@ -13,6 +13,7 @@ class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZI
public:
typedef struct {
s32 default_torque;
s32 default_velocity;
} flash_config_t;
private:
@ -29,26 +30,26 @@ class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZI
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) override;
virtual int32_t stop(u8 stop_type) override;
virtual int32_t position_calibrate(s32 nowpos) override;
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) override;
virtual int32_t move_to(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) override;
virtual int32_t move_by(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) override;
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 torque);
virtual int32_t move_backward(s32 torque);
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) override;
virtual int32_t move_by_nolimit(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) override;
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) override;
virtual int32_t read_status(status_t &status) override;
virtual int32_t read_detailed_status(detailed_status_t &debug_info) override;
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) override;
virtual int32_t get_basic_param(basic_param_t &param) override;
virtual int32_t set_basic_param(basic_param_t &param);
virtual int32_t get_basic_param(basic_param_t &param);
/*******************************************************************************
* OVERRIDE MODULE *

70
components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.cpp
View File

@ -2,72 +2,6 @@
#include "scirpt_cmder_mini_servo_motor_ctrl_module.hpp"
#include <string.h>
#if 0
// #include "sdk\components\zprotocols\zcancmder\zcancmder_protocol.hpp"
using namespace iflytop;
#define TAG "CMD"
static void cmd_dump_ack(I_MiniServoModule::version_t& ack) { //
ZLOGI(TAG, "version:%d", ack.version);
}
static void cmd_dump_ack(I_MiniServoModule::status_t& ack) { //
ZLOGI(TAG, "status :%d", ack.status);
ZLOGI(TAG, "ioState :%d", ack.ioState);
ZLOGI(TAG, "torque :%d", ack.torque);
ZLOGI(TAG, "speed :%d", ack.speed);
ZLOGI(TAG, "pos :%d", ack.pos);
}
static void cmd_dump_ack(I_MiniServoModule::detailed_status_t& ack) { //
ZLOGI(TAG, "status :%d", ack.status);
ZLOGI(TAG, "ioState :%d", ack.ioState);
ZLOGI(TAG, "torque :%d", ack.torque);
ZLOGI(TAG, "speed :%d", ack.speed);
ZLOGI(TAG, "pos :%d", ack.pos);
}
static void cmd_dump_ack(I_MiniServoModule::basic_param_t& ack) { //
ZLOGI(TAG, "posCalibrate: %d", ack.posCalibrate);
ZLOGI(TAG, "minlimit: %d", ack.minlimit);
ZLOGI(TAG, "maxlimit: %d", ack.maxlimit);
ZLOGI(TAG, "maxtemp: %d", ack.maxtemp);
ZLOGI(TAG, "minvoltage: %d", ack.minvoltage);
ZLOGI(TAG, "maxvoltage: %d", ack.maxvoltage);
ZLOGI(TAG, "protectCurrent: %d", ack.protectCurrent);
ZLOGI(TAG, "protectTorque: %d", ack.protectTorque);
}
void ScirptCmderMiniServoMotorCtrlModule::initialize(CmdSchedulerV2* cmdScheduler) {
m_cmdScheduler = cmdScheduler;
ZASSERT(m_cmdScheduler != nullptr);
regcmd();
}
int32_t ScirptCmderMiniServoMotorCtrlModule::findmodule(int id, I_MiniServoModule** module) {
auto it = moduler.find(id);
if (it == moduler.end()) {
return err::kmodule_not_found;
}
*module = it->second;
return 0;
}
//static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; }
void ScirptCmderMiniServoMotorCtrlModule::regmodule(int id, I_MiniServoModule* robot) {
ZASSERT(moduler.find(id) == moduler.end());
ZASSERT(robot != nullptr);
moduler[id] = robot;
}
void ScirptCmderMiniServoMotorCtrlModule::regcmd() {
REG_CMD___NO_ACK("mini_servo_", enable, "(id,en)", 2, con.getBool(2));
REG_CMD___NO_ACK("mini_servo_", stop, "(id,stop_type)", 2, con.getInt(2));
REG_CMD___NO_ACK("mini_servo_", position_calibrate, "(id,pos)", 2, con.getInt(2));
REG_CMD___NO_ACK("mini_servo_", rotate, "(id,speed,torque,time)", 4, con.getInt(2), con.getInt(3), con.getInt(4), [this](int32_t ecode) { ZLOGI(TAG, "mini_servo_rotate done %d", ecode); });
REG_CMD___NO_ACK("mini_servo_", move_to, "(id,pos,speed,torque)", 4, con.getInt(2), con.getInt(3), con.getInt(4), [this](int32_t ecode) { ZLOGI(TAG, "mini_servo_move_to done %d", ecode); });
REG_CMD___NO_ACK("mini_servo_", move_by, "(id,pos,speed,torque)", 4, con.getInt(2), con.getInt(3), con.getInt(4), [this](int32_t ecode) { ZLOGI(TAG, "mini_servo_move_by done %d", ecode); });
REG_CMD___NO_ACK("mini_servo_", move_forward, "(id,torque)", 2, con.getInt(2));
REG_CMD___NO_ACK("mini_servo_", move_backward, "(id,torque)", 2, con.getInt(2));
REG_CMD_WITH_ACK("mini_servo_", read_version, "(id)", 1, I_MiniServoModule::version_t, ack);
REG_CMD_WITH_ACK("mini_servo_", read_status, "(id)", 1, I_MiniServoModule::status_t, ack);
REG_CMD_WITH_ACK("mini_servo_", read_detailed_status, "(id)", 1, I_MiniServoModule::detailed_status_t, ack);
REG_CMD_WITH_ACK("mini_servo_", get_basic_param, "(id)", 1, I_MiniServoModule::basic_param_t, ack);
}
#endif

22
components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.hpp
View File

@ -1,23 +1,5 @@
#pragma once
#include <map>
#if 0
#include "sdk/os/zos.hpp"
#include "sdk\components\cmdscheduler\cmd_scheduler_v2.hpp"
#include "sdk\components\zprotocols\zcancmder\api\i_mini_servo_module.hpp"
namespace iflytop {
class ScirptCmderMiniServoMotorCtrlModule {
CmdSchedulerV2* m_cmdScheduler;
map<uint8_t, I_MiniServoModule*> moduler;
I_MiniServoModule* module;
public:
void initialize(CmdSchedulerV2* cmdScheduler);
void regmodule(int id, I_MiniServoModule* robot);
void regcmd();
private:
int32_t findmodule(int id, I_MiniServoModule** module);
};
} // namespace iflytop
#endif

64
components/zcancmder_module/zcan_mini_servo_ctrl_module.cpp
View File

@ -1,69 +1,5 @@
#include "zcan_mini_servo_ctrl_module.hpp"
using namespace iflytop;
#if 0
#define TAG "ZCanMiniServoCtrlModule"
void ZCanMiniServoCtrlModule::initialize(ZCanCmder* cancmder, int id, I_MiniServoModule* module) {
m_cancmder = cancmder;
m_id = id;
m_module = module;
m_lock.init();
cancmder->registerListener(this);
}
void ZCanMiniServoCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
PROCESS_PACKET(kcmd_mini_servo_ctrl_enable, m_id) { errorcode = m_module->enable(cmd->enable); }
END_PP();
PROCESS_PACKET(kcmd_mini_servo_ctrl_stop, m_id) { errorcode = m_module->stop(cmd->stop_type); }
END_PP();
PROCESS_PACKET(kcmd_mini_servo_ctrl_position_calibrate, m_id) { errorcode = m_module->position_calibrate(cmd->calibrate_pos); }
END_PP();
PROCESS_PACKET(kcmd_mini_servo_ctrl_rotate, m_id) { //
errorcode = m_module->rotate(cmd->speed, cmd->torque, cmd->run_time, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_mini_servo_ctrl_rotate); });
}
END_PP();
PROCESS_PACKET(kcmd_mini_servo_ctrl_move_to, m_id) { //
errorcode = m_module->move_to(cmd->pos, cmd->speed, cmd->torque, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_mini_servo_ctrl_move_to); });
}
END_PP();
PROCESS_PACKET(kcmd_mini_servo_ctrl_move_by, m_id) { //
errorcode = m_module->move_by(cmd->pos, cmd->speed, cmd->torque, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_mini_servo_ctrl_move_by); });
}
END_PP();
PROCESS_PACKET(kcmd_mini_servo_ctrl_run_with_torque, m_id) { //
errorcode = m_module->run_with_torque(cmd->torque, cmd->run_time, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_mini_servo_ctrl_run_with_torque); });
}
END_PP();
PROCESS_PACKET(kcmd_mini_servo_ctrl_move_by_nolimit, m_id) { //
errorcode = m_module->move_by_nolimit(cmd->pos, cmd->speed, cmd->torque, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_mini_servo_ctrl_move_by_nolimit); });
}
END_PP();
PROCESS_PACKET(kcmd_mini_servo_ctrl_read_version, m_id) { errorcode = m_module->read_version(ack->ack); }
END_PP();
PROCESS_PACKET(kcmd_mini_servo_ctrl_read_status, m_id) { errorcode = m_module->read_status(ack->ack); }
END_PP();
PROCESS_PACKET(kcmd_mini_servo_ctrl_read_detailed_status, m_id) { errorcode = m_module->read_detailed_status(ack->ack); }
END_PP();
PROCESS_PACKET(kcmd_mini_servo_ctrl_set_basic_param, m_id) { errorcode = m_module->set_basic_param(cmd->param); }
END_PP();
PROCESS_PACKET(kcmd_mini_servo_ctrl_get_basic_param, m_id) { errorcode = m_module->get_basic_param(ack->ack); }
END_PP();
}
#endif

14
components/zcancmder_module/zcan_mini_servo_ctrl_module.hpp
View File

@ -2,19 +2,5 @@
#include "sdk/components/zprotocols/zcancmder/api/i_mini_servo_module.hpp"
#include "sdk\components\zcancmder\zcanreceiver.hpp"
namespace iflytop {
#if 0
class ZCanMiniServoCtrlModule : public ZCanCmderListener {
ZCanCmder* m_cancmder = nullptr;
int m_id = 0;
I_MiniServoModule* m_module;
uint8_t m_txbuf[128] = {0};
zmutex m_lock;
public:
void initialize(ZCanCmder* cancmder, int id, I_MiniServoModule* module);
virtual void onRceivePacket(CanPacketRxBuffer* rxcmd);
};
#endif
} // namespace iflytop

2
components/zprotocols/zcancmder

@ -1 +1 @@
Subproject commit b03e0a12e3ae450cbcdf5fd0f0218b258071f2eb
Subproject commit 9c2f754b924618706274d948dcda39b5d9282fce
Write Preview
Loading…
Cancel
Save