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 2 years ago
parent
6d563c1930
commit
ed76b5bd57
6 changed files with 131 additions and 27 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. 4
      components/cmdscheduler/cmd_scheduler.cpp
  2. 4
      components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp
  3. 8
      components/mini_servo_motor/feite_servo_motor.hpp
  4. 81
      components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
  5. 59
      components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp
  6. 2
      components/zprotocols/zcancmder_v2

4
components/cmdscheduler/cmd_scheduler.cpp
View File

@ -133,7 +133,7 @@ int32_t CmdScheduler::callcmd(const char* cmd) {
void CmdScheduler::remove_note(char* input, int inputlen) {
bool detect_note = false;
for (size_t i = 0; i < inputlen; i++) {
for (int i = 0; i < inputlen; i++) {
if (!detect_note && input[i] == '#') {
detect_note = true;
}
@ -144,7 +144,7 @@ void CmdScheduler::remove_note(char* input, int inputlen) {
}
void CmdScheduler::prase_cmd(char* input, int inputlen, int& argc, char* argv[]) {
for (size_t i = 0; input[i] == 0 || i < inputlen; i++) {
for (int i = 0; input[i] == 0 || i < inputlen; i++) {
if (input[i] == ' ' || input[i] == '\r' || input[i] == '\n') {
input[i] = 0;
}

4
components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp
View File

@ -48,7 +48,7 @@ static void cmd_dump_ack(Eq20ServoMotor::servo_internal_status_t ack) { //
ZLOGI(TAG, "encoder_battery_warning:%d", ack.data.sbit.encoder_battery_warning);
ZLOGI(TAG, "encoder_battery_report :%d", ack.data.sbit.encoder_battery_report);
}
static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; }
//static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; }
void ScriptCmderEq20Servomotor::regmodule(int id, Eq20ServoMotor* robot) {
ZASSERT(moduler.find(id) == moduler.end());
@ -77,4 +77,4 @@ void ScriptCmderEq20Servomotor::regcmd() {
REG_CMD_WITH_ACK("eq20_", read_reg, "(id,regadd)", 2, int32_t, con->getInt(2), ack);
REG_CMD___NO_ACK("eq20_", write_reg_bit, "(id,regadd,off,value)", 4, con->getInt(2), con->getInt(3), con->getInt(4));
REG_CMD_WITH_ACK("eq20_", read_reg_bit, "(id,regadd,off)", 3, int32_t, con->getInt(2), con->getInt(3), ack);
}
}

8
components/mini_servo_motor/feite_servo_motor.hpp
View File

@ -141,6 +141,12 @@ class FeiTeServoMotor {
bool getTorqueSwitch(uint8_t id, bool& on);
bool getNowPos(uint8_t id, int16_t& pos);
bool getNowPos(uint8_t id, int32_t& pos) {
int16_t pos16;
bool ret = getNowPos(id, pos16);
pos = pos16;
return ret;
}
bool setTargetPos(uint8_t id, int16_t pos);
bool getServoCalibration(uint8_t, int16_t& poscalibration);
@ -178,7 +184,7 @@ class FeiTeServoMotor {
void dump_detailed_status(detailed_status_t* detailed_status);
bool getMoveFlag(uint8_t id, uint8_t& moveflag);
public:
bool write_u8(uint8_t id, feite::reg_add_e add, uint8_t regval);
bool write_u16(uint8_t id, feite::reg_add_e add, uint16_t regval);

81
components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
View File

@ -5,14 +5,16 @@ using namespace iflytop;
using namespace std;
#define TAG "MiniRobotCtrlModule"
static void limit_input(s32& input, s32 min, s32 max) {
static void limit_input(s32 &input, s32 min, s32 max) {
if (input > max) input = max;
if (input < min) input = min;
}
void MiniRobotCtrlModule::initialize(FeiTeServoMotor* bus, uint8_t id) {
m_bus = bus;
m_id = id;
void MiniRobotCtrlModule::initialize(uint16_t module_id, FeiTeServoMotor *bus, uint8_t idinbus) {
m_bus = bus;
m_id = idinbus;
m_module_id = module_id;
m_bus->write_u8(m_id, feite::kRegServoRunMode, feite::kServoMode);
m_thread.init("MiniRobotCtrlModule", 1024, osPriorityNormal);
}
@ -201,13 +203,13 @@ int32_t MiniRobotCtrlModule::run_with_torque(s32 torque, s32 run_time, action_cb
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) {
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;
}
int32_t MiniRobotCtrlModule::read_status(status_t& status) {
int32_t MiniRobotCtrlModule::read_status(status_t &status) {
if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
FeiTeServoMotor::detailed_status_t feitestatus;
@ -223,7 +225,7 @@ int32_t MiniRobotCtrlModule::read_status(status_t& status) {
}
return 0;
}
int32_t MiniRobotCtrlModule::read_detailed_status(detailed_status_t& debug_info) {
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;
@ -244,8 +246,8 @@ int32_t MiniRobotCtrlModule::read_detailed_status(detailed_status_t& debug_info)
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) {
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, "get_basic_param");
if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
m_bus->read_u16(m_id, feite::kRegServoCalibration, param.posCalibrate);
@ -263,3 +265,64 @@ void MiniRobotCtrlModule::call_status_cb(action_cb_status_t cb, int32_t status)
m_last_exec_status = status;
if (cb) cb(status);
}
/*******************************************************************************
* OVERRIDE MODULE *
*******************************************************************************/
int32_t MiniRobotCtrlModule::getid(int32_t *id) {
*id = m_module_id;
return 0;
}
int32_t MiniRobotCtrlModule::module_stop() { return stop(0); }
int32_t MiniRobotCtrlModule::module_break() { return stop(0); }
int32_t MiniRobotCtrlModule::module_get_last_exec_status(int32_t *status) {
*status = m_last_exec_status;
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) {
/**
* @brief TODO:
* Ìí¼Ó¹ýÔؼì²â
*/
*iserror = 0;
return 0;
}
int32_t MiniRobotCtrlModule::module_clear_error() { return 0; }
int32_t MiniRobotCtrlModule::module_set_param(int32_t param_id, int32_t param_value) { return err::kmodule_not_find_config_index; }
int32_t MiniRobotCtrlModule::module_get_param(int32_t param_id, int32_t *param_value) { return err::kmodule_not_find_config_index; }
int32_t MiniRobotCtrlModule::module_readio(int32_t *io) {
*io = 0;
return 0;
}
int32_t MiniRobotCtrlModule::module_writeio(int32_t io) { return 0; }
int32_t MiniRobotCtrlModule::module_read_adc(int32_t adcindex, int32_t *adc) {
*adc = 0;
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_move_to_with_torque(int32_t pos, int32_t torque) { return move_to(pos, 0, torque, nullptr); }
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;
}

59
components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp
View File

@ -4,18 +4,20 @@
//
#include "feite_servo_motor.hpp"
#include "sdk/components/zprotocols/zcancmder/api/i_mini_servo_module.hpp"
#include "sdk\components\zprotocols\zcancmder_v2\api\api.hpp"
namespace iflytop {
class MiniRobotCtrlModule : public I_MiniServoModule {
FeiTeServoMotor* m_bus;
class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZIMotor {
FeiTeServoMotor *m_bus;
uint8_t m_id;
uint16_t m_module_id;
ZThread m_thread;
s32 m_pos_shift;
s32 m_last_exec_status = 0;
public:
void initialize(FeiTeServoMotor* bus, uint8_t id);
void initialize(uint16_t module_id, FeiTeServoMotor *bus, uint8_t idinbus);
virtual int32_t enable(u8 enable) override;
virtual int32_t stop(u8 stop_type) override;
@ -31,17 +33,50 @@ class MiniRobotCtrlModule : public I_MiniServoModule {
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 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) 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 set_run_param(run_param_t& param) override;
virtual int32_t set_basic_param(basic_param_t& param) override;
// virtual int32_t set_warning_limit_param(warning_limit_param_t& param) override;
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 get_run_param(run_param_t& param) override;
virtual int32_t get_basic_param(basic_param_t& param) override;
// virtual int32_t get_warning_limit_param(warning_limit_param_t& param) override;
/*******************************************************************************
* OVERRIDE MODULE *
*******************************************************************************/
virtual int32_t getid(int32_t *id) 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;
virtual int32_t module_set_param(int32_t param_id, int32_t param_value) override;
virtual int32_t module_get_param(int32_t param_id, int32_t *param_value) override;
virtual int32_t module_readio(int32_t *io) override;
virtual int32_t module_writeio(int32_t io) override;
virtual int32_t module_read_adc(int32_t adcindex, int32_t *adc) override;
/*******************************************************************************
* Motor *
*******************************************************************************/
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_move_to_with_torque(int32_t pos, int32_t torque) override;
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;
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;
virtual int32_t motor_read_pos(int32_t *pos);
private:
void call_status_cb(action_cb_status_t cb, int32_t status);

2
components/zprotocols/zcancmder_v2

@ -1 +1 @@
Subproject commit 115dbcfc41877dd4ceb3412df2838322ce7f09e2
Subproject commit 5abf00ddaeca30ef08bad4b8b69ccfa7b074d161
Write Preview
Loading…
Cancel
Save