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

add step45 motor driver

master
zhaohe 2 years ago
parent
108101c247
commit
e355909864
10 changed files with 145 additions and 82 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. 2
      components/cmdscheduler/cmd_scheduler.cpp
  2. 4
      components/cmdscheduler/cmd_scheduler.hpp
  3. 24
      components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.cpp
  4. 51
      components/step_motor_45/script_cmder_step_motor_45.cpp
  5. 5
      components/step_motor_45/script_cmder_step_motor_45.hpp
  6. 0
      components/step_motor_45/script_cmder_step_motor_45_scheduler.cpp
  7. 32
      components/step_motor_45/step_motor_45.cpp
  8. 35
      components/step_motor_45/step_motor_45.hpp
  9. 34
      components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp

2
components/cmdscheduler/cmd_scheduler.cpp
View File

@ -68,7 +68,7 @@ void CmdScheduler::schedule() {
if (!m_dataisready) {
return;
}
for (size_t i = 0; i < m_rxsize; i++) {
for (int i = 0; i < m_rxsize; i++) {
if (rxbuf[i] == '\r' || rxbuf[i] == '\n') {
rxbuf[i] = '\0';
}

4
components/cmdscheduler/cmd_scheduler.hpp
View File

@ -98,12 +98,12 @@ class CmdScheduler {
cmd_dump_ack(ack); \
return (int32_t)0;
#define REG_CMD(prefix, cmd, para, npara, ...) /**/ \
#define REG_CMD___NO_ACK(prefix, cmd, para, npara, ...) /**/ \
m_cmdScheduler->registerCmd(prefix #cmd, para, npara, [this](CmdScheduler::Context* con) { /**/ \
IMPL_CMD(cmd, __VA_ARGS__); /**/ \
});
#define REG_DUMP_ACK_CMD(prefix, cmd, para, npara, acktype, ...) /**/ \
#define REG_CMD_WITH_ACK(prefix, cmd, para, npara, acktype, ...) /**/ \
m_cmdScheduler->registerCmd(prefix #cmd, para, npara, [this](CmdScheduler::Context* con) { /**/ \
acktype ack; /**/ \
IMPL_READ_STATE(cmd, __VA_ARGS__); /**/ \

24
components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.cpp
View File

@ -58,16 +58,16 @@ void ScirptCmderMiniServoMotorCtrlModule::regmodule(int id, I_MiniServoModule* r
}
void ScirptCmderMiniServoMotorCtrlModule::regcmd() {
REG_CMD("mini_servo_", enable, "(id,en)", 2, con->getBool(2));
REG_CMD("mini_servo_", stop, "(id,stop_type)", 2, con->getInt(2));
REG_CMD("mini_servo_", position_calibrate, "(id,pos)", 2, con->getInt(2));
REG_CMD("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("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("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("mini_servo_", move_forward, "(id,torque)", 2, con->getInt(2));
REG_CMD("mini_servo_", move_backward, "(id,torque)", 2, con->getInt(2));
REG_DUMP_ACK_CMD("mini_servo_", read_version, "(id)", 1, I_MiniServoModule::version_t, ack);
REG_DUMP_ACK_CMD("mini_servo_", read_status, "(id)", 1, I_MiniServoModule::status_t, ack);
REG_DUMP_ACK_CMD("mini_servo_", read_detailed_status, "(id)", 1, I_MiniServoModule::detailed_status_t, ack);
REG_DUMP_ACK_CMD("mini_servo_", get_basic_param, "(id)", 1, I_MiniServoModule::basic_param_t, ack);
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);
}

51
components/step_motor_45/script_cmder_step_motor_45.cpp
View File

@ -0,0 +1,51 @@
#include "script_cmder_step_motor_45.hpp"
#include <string.h>
// #include "sdk\components\zprotocols\zcancmder\zcancmder_protocol.hpp"
using namespace iflytop;
#define TAG "CMD"
void ScriptCmderStepMotor45::initialize(CmdScheduler* cmdScheduler) {
m_cmdScheduler = cmdScheduler;
ZASSERT(m_cmdScheduler != nullptr);
regcmd();
}
int32_t ScriptCmderStepMotor45::findmodule(int id, StepMotor45** module) {
auto it = moduler.find(id);
if (it == moduler.end()) {
return err::kce_no_such_module;
}
*module = it->second;
return 0;
}
// static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; }
void ScriptCmderStepMotor45::regmodule(int id, StepMotor45* robot) {
ZASSERT(moduler.find(id) == moduler.end());
ZASSERT(robot != nullptr);
moduler[id] = robot;
}
static void cmd_dump_ack(int32_t& ack) { ZLOGI(TAG, "ack %d", ack); }
static void cmd_dump_ack(bool& ack) { ZLOGI(TAG, "ack %d", ack); }
void ScriptCmderStepMotor45::regcmd() {
#define PREFIX "step_motor_45_"
REG_CMD___NO_ACK(PREFIX, rotate, "(id,direction)", 2, con->getInt(2));
REG_CMD___NO_ACK(PREFIX, move_by, "(id,pos)", 2, con->getInt(2));
REG_CMD___NO_ACK(PREFIX, move_to, "(id,pos)", 2, con->getInt(2));
REG_CMD___NO_ACK(PREFIX, set_default_speed, "(id,speed)", 2, con->getInt(2));
REG_CMD_WITH_ACK(PREFIX, get_default_speed, "(id)", 1, int32_t, ack);
REG_CMD_WITH_ACK(PREFIX, is_reach_target_pos, "(id)", 1, bool, ack);
REG_CMD___NO_ACK(PREFIX, stop, "(id)", 1);
REG_CMD___NO_ACK(PREFIX, zero_calibration, "(id)", 1);
REG_CMD_WITH_ACK(PREFIX, get_pos, "(id)", 1, int32_t, ack);
REG_CMD___NO_ACK(PREFIX, set_pos, "(id,pos)", 2, con->getInt(2));
// REG_CMD___NO_ACK(PREFIX, initialize, "(id,driverPin1,driverPin2,driverPin3,driverPin4,zeroPin)", 6, con->getInt(2), con->getInt(3), con->getInt(4), con->getInt(5), con->getInt(6), con->getInt(7));
}

5
components/step_motor_45/script_cmder_step_motor_45_scheduler.hpp → components/step_motor_45/script_cmder_step_motor_45.hpp
View File

@ -6,9 +6,10 @@
#include "step_motor_45.hpp"
namespace iflytop {
class ScriptCmderStepMotor45Scheduler {
class ScriptCmderStepMotor45 {
CmdScheduler* m_cmdScheduler;
map<uint8_t, StepMotor45*> moduler;
StepMotor45* module;
public:
void initialize(CmdScheduler* cmdScheduler);
@ -16,7 +17,7 @@ class ScriptCmderStepMotor45Scheduler {
void regcmd();
private:
StepMotor45* findModule(int id);
int32_t findmodule(int id, StepMotor45** module);
};
} // namespace iflytop

0
components/step_motor_45/script_cmder_step_motor_45_scheduler.cpp
View File

32
components/step_motor_45/step_motor_45.cpp
View File

@ -57,11 +57,11 @@ void StepMotor45::initialize(cfg_t cfg) {
return;
}
void StepMotor45::rotate(int direction, int speed) {
int32_t StepMotor45::rotate(int direction, int speed) {
CriticalContext cc;
if (direction == 0) {
stop();
return;
return (int32_t)0;
}
if (direction > 0) {
@ -73,20 +73,23 @@ void StepMotor45::rotate(int direction, int speed) {
posmode = false;
m_state = direction > 0 ? kforward_rotation : krollback;
ZLOGI(TAG, "rotate %d", direction);
return (int32_t)0;
}
void StepMotor45::stop() {
int32_t StepMotor45::stop() {
CriticalContext cc;
stop_motor();
return (int32_t)0;
}
void StepMotor45::zeroCalibration() {
int32_t StepMotor45::zero_calibration() {
CriticalContext cc;
if (getzeropinstate()) {
m_calibration = true;
m_pos = 0;
return;
return (int32_t)0;
}
m_exec_zero_calibration_task = true;
m_state = krollback;
return (int32_t)0;
}
void StepMotor45::onTimIRQ1ms() {
@ -197,11 +200,11 @@ void StepMotor45::schedule() {
m_pos += direction;
}
void StepMotor45::moveBy(int bypos, int speed) {
int32_t StepMotor45::move_by(int bypos, int speed) {
CriticalContext cc;
if (bypos == 0) {
m_targetPos = m_pos;
return;
return (int32_t)0;
}
if (bypos > 0) {
m_state = kforward_rotation;
@ -211,13 +214,14 @@ void StepMotor45::moveBy(int bypos, int speed) {
m_targetPos = bypos + m_pos;
posmode = true;
return (int32_t)0;
}
void StepMotor45::moveTo(int to, int speed) {
int32_t StepMotor45::move_to(int to, int speed) {
CriticalContext cc;
if (to == m_pos) {
return;
return (int32_t)0;
}
if (to <= m_pos) {
@ -228,15 +232,15 @@ void StepMotor45::moveTo(int to, int speed) {
m_targetPos = to;
posmode = true;
return (int32_t)0;
}
void StepMotor45::moveTo(int to) { moveTo(to, defaultspeed); }
bool StepMotor45::isReachTargetPos() {
int32_t StepMotor45::move_to(int to) { move_to(to, defaultspeed); }
int32_t StepMotor45::is_reach_target_pos(bool& reach) {
CriticalContext cc;
if (posmode) {
return m_pos == m_targetPos;
reach = m_pos == m_targetPos;
}
return true;
return (int32_t)0;
}
bool StepMotor45::getzeropinstate() {

35
components/step_motor_45/step_motor_45.hpp
View File

@ -61,20 +61,27 @@ class StepMotor45 {
public:
void initialize(cfg_t cfg);
void rotate(int direction) { rotate(direction, defaultspeed); }
void rotate(int direction, int speed);
void moveBy(int pos, int speed);
void moveBy(int pos) { moveBy(pos, defaultspeed); }
void moveTo(int to, int speed);
void moveTo(int to);
void setDefaultSpeed(int speed) { defaultspeed = speed; }
bool isReachTargetPos();
void stop();
void zeroCalibration();
void setPos(int pos) { m_pos = pos; }
int getPos() { return m_pos; }
int32_t rotate(int direction) { rotate(direction, defaultspeed); }
int32_t rotate(int direction, int speed);
int32_t move_by(int pos, int speed);
int32_t move_by(int pos) { move_by(pos, defaultspeed); }
int32_t move_to(int to, int speed);
int32_t move_to(int to);
int32_t set_default_speed(int speed) { defaultspeed = speed; }
int32_t get_default_speed(int32_t &speed) {
speed = defaultspeed;
return (int32_t)0;
}
int32_t is_reach_target_pos(bool &reach);
int32_t stop();
int32_t zero_calibration();
int32_t set_pos(int32_t pos) { m_pos = pos; }
int32_t get_pos(int32_t &pos) {
pos = m_pos;
return (int32_t)0;
}
private:
bool getzeropinstate();

34
components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp
View File

@ -73,23 +73,23 @@ void XYRobotScriptCmderModule::regcmd() { //
#define PREIX "xy_robot_ctrl_"
REG_CMD(PREIX, enable, "(id,en)", 2, con->getBool(2));
REG_CMD(PREIX, stop, "(id,stop_type)", 2, con->getInt(2));
REG_CMD(PREIX, move_to_zero, "(id)", 1, [this](int32_t status) { ZLOGI(TAG, "move_to_zero status:%d", status); });
REG_CMD(PREIX, move_to_zero_with_calibrate, "(id,nowx,nowy)", 3, con->getInt(2), con->getInt(3), [this](int32_t status) { ZLOGI(TAG, "move_to_zero_with_calibrate status:%d", status); });
REG_CMD(PREIX, move_to, "(id,x,y,speed)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_to status:%d", status); });
REG_CMD(PREIX, move_by, "(id,dx,dy,v)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); });
REG_CMD(PREIX, force_change_current_pos, "(id,x,y)", 3, con->getInt(2), con->getInt(3));
REG_DUMP_ACK_CMD(PREIX, read_version, "(id)", 1, I_XYRobotCtrlModule::version_t, ack);
REG_DUMP_ACK_CMD(PREIX, read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack);
REG_DUMP_ACK_CMD(PREIX, read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack);
REG_DUMP_ACK_CMD(PREIX, get_base_param, "(id)", 1, I_XYRobotCtrlModule::base_param_t, ack);
REG_DUMP_ACK_CMD("xy_robot_ctrl_", read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack);
REG_DUMP_ACK_CMD("xy_robot_ctrl_", read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack);
REG_DUMP_ACK_CMD("xy_robot_ctrl_", read_version, "(id)", 1, I_XYRobotCtrlModule::version_t, ack);
REG_CMD("xy_robot_ctrl_", flush, "(id)", 1);
REG_CMD("xy_robot_ctrl_", factory_reset, "(id)", 1);
REG_CMD("xy_robot_ctrl_", move_by_no_limit, "(id,dx,dy,v)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_by_no_limit status:%d", status); });
REG_CMD___NO_ACK(PREIX, enable, "(id,en)", 2, con->getBool(2));
REG_CMD___NO_ACK(PREIX, stop, "(id,stop_type)", 2, con->getInt(2));
REG_CMD___NO_ACK(PREIX, move_to_zero, "(id)", 1, [this](int32_t status) { ZLOGI(TAG, "move_to_zero status:%d", status); });
REG_CMD___NO_ACK(PREIX, move_to_zero_with_calibrate, "(id,nowx,nowy)", 3, con->getInt(2), con->getInt(3), [this](int32_t status) { ZLOGI(TAG, "move_to_zero_with_calibrate status:%d", status); });
REG_CMD___NO_ACK(PREIX, move_to, "(id,x,y,speed)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_to status:%d", status); });
REG_CMD___NO_ACK(PREIX, move_by, "(id,dx,dy,v)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); });
REG_CMD___NO_ACK(PREIX, force_change_current_pos, "(id,x,y)", 3, con->getInt(2), con->getInt(3));
REG_CMD_WITH_ACK(PREIX, read_version, "(id)", 1, I_XYRobotCtrlModule::version_t, ack);
REG_CMD_WITH_ACK(PREIX, read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack);
REG_CMD_WITH_ACK(PREIX, read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack);
REG_CMD_WITH_ACK(PREIX, get_base_param, "(id)", 1, I_XYRobotCtrlModule::base_param_t, ack);
REG_CMD_WITH_ACK("xy_robot_ctrl_", read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack);
REG_CMD_WITH_ACK("xy_robot_ctrl_", read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack);
REG_CMD_WITH_ACK("xy_robot_ctrl_", read_version, "(id)", 1, I_XYRobotCtrlModule::version_t, ack);
REG_CMD___NO_ACK("xy_robot_ctrl_", flush, "(id)", 1);
REG_CMD___NO_ACK("xy_robot_ctrl_", factory_reset, "(id)", 1);
REG_CMD___NO_ACK("xy_robot_ctrl_", move_by_no_limit, "(id,dx,dy,v)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_by_no_limit status:%d", status); });
m_cmdScheduler->registerCmd("xy_robot_ctrl_set_base_param", "(id,paramName,val)", 3, [this](CmdScheduler::Context* con) {
const char* paramName = con->getString(2);

Write Preview
Loading…
Cancel
Save