diff --git a/sdk b/sdk index 3307f7a..2b35686 160000 --- a/sdk +++ b/sdk @@ -1 +1 @@ -Subproject commit 3307f7a4c801857b8cd9cab5abb290f88ffc3155 +Subproject commit 2b35686d8d692c80c80bdbb6b7e4afa38d94f860 diff --git a/usrc/feite_servo_motor.cpp b/usrc/feite_servo_motor.cpp index eee3cd6..7171057 100644 --- a/usrc/feite_servo_motor.cpp +++ b/usrc/feite_servo_motor.cpp @@ -17,11 +17,13 @@ using namespace feite; } static void dumphex(const char* tag, uint8_t* data, uint8_t len) { + #if 1 printf("%s:", tag); for (int i = 0; i < len; i++) { printf("%02x ", data[i]); } printf("\n"); + #endif } void FeiTeServoMotor::initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx) { m_uart = uart; @@ -262,7 +264,7 @@ bool FeiTeServoMotor::write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbi return write_u16(id, add, val); } bool FeiTeServoMotor::write_reg(uint8_t id, bool async, uint8_t add, uint8_t* data, uint8_t len) { // - ZLOGI(TAG, "write_reg id:%d add:%d len:%d", id, add, len); + // ZLOGI(TAG, "write_reg id:%d add:%d len:%d", id, add, len); cmd_header_t* cmd_header = (cmd_header_t*)m_txbuf; receipt_header_t* receipt_header = (receipt_header_t*)m_rxbuf; cmd_header->header = 0xffff; diff --git a/usrc/main.cpp b/usrc/main.cpp index 2d90f37..8c69e31 100644 --- a/usrc/main.cpp +++ b/usrc/main.cpp @@ -27,7 +27,7 @@ using namespace std; return; \ } extern "C" { -void StartDefaultTask(void const* argument) { umain(); } +void StartDefaultTask(void const* argument) { iflytop::gmain.run(); } } /******************************************************************************* * 配置 * @@ -116,309 +116,21 @@ static StepMotor45::cfg_t cfg6 = { .driverPinMirror = true, }; -map screw_lead = { - {1, 10.0}, // - {2, 10.0}, // - {3, 10.0}, // - {4, 10.0}, // - {5, 10.0}, // - {6, 10.0}, // -}; - /******************************************************************************* * 代码 * *******************************************************************************/ - -static StepMotor45 g_step_motor45[7]; -StepMotor g_step_motor[10]; -IflytopCanMaster m_IflytopCanMaster; -static CmdScheduler cmdScheduler; +namespace iflytop { +StepMotor45 g_step_motor45[7]; +StepMotor g_step_motor[10]; +IflytopCanMaster m_IflytopCanMaster; +CmdScheduler cmdScheduler; ModbusBlockHost g_modbusblockhost; Eq20ServoMotor g_eq20servomotor; FeiTeServoMotor g_feiteservomotor; - -bool distance_mm_to_step(int motorid, float distance_mm, int32_t* distance) { - if (screw_lead.find(motorid) == screw_lead.end()) { - return false; - } - float lead = screw_lead[motorid]; - *distance = distance_mm / lead * 51200; - return true; -} +} // namespace iflytop void regfn() { - cmdScheduler.registerCmd("help", // - [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { ZLOGI(TAG, "do_help"); }); - cmdScheduler.registerCmd("reset_board", // - [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { NVIC_SystemReset(); }); - cmdScheduler.registerCmd("stepmotor45_wait_to_reach_pos", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - // stepmotor45_rotate motorid direction - CHECK_ARGC(2); - int motorid = atoi(argv[1]); - - if (motorid < 1 || motorid > 6) { - ZLOGE(TAG, "motorid out of range"); - return; - } - while (true) { - if (g_step_motor45[motorid].isReachTargetPos()) { - break; - } - ZLOGI(TAG, "stepmotor45_wait_to_reach_pos %d", motorid); - osDelay(100); - } - }); - cmdScheduler.registerCmd("stepmotor45_reset_pos_all", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - // stepmotor45_rotate motorid direction - CHECK_ARGC(0); - g_step_motor45[1].setPos(0); - g_step_motor45[2].setPos(0); - g_step_motor45[3].setPos(0); - g_step_motor45[4].setPos(0); - g_step_motor45[5].setPos(0); - g_step_motor45[6].setPos(0); - }); - - // stepmotor45 - cmdScheduler.registerCmd("stepmotor45_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - // stepmotor45_rotate motorid direction - CHECK_ARGC(2); - int motorid = atoi(argv[1]); - int direction = atoi(argv[2]); - - if (motorid < 1 || motorid > 6) { - ZLOGE(TAG, "motorid out of range"); - return; - } - - g_step_motor45[motorid].rotate(direction); - }); - cmdScheduler.registerCmd("stepmotor45_readPos", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - // stepmotor45_rotate motorid direction - CHECK_ARGC(2); - int motorid = atoi(argv[1]); - - if (motorid < 1 || motorid > 6) { - ZLOGE(TAG, "motorid out of range"); - return; - } - - ZLOGI(TAG, "motorid %d pos %d", motorid, g_step_motor45[motorid].getPos()); - }); - - cmdScheduler.registerCmd("stepmotor45_moveTo", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(2); - int motorid = atoi(argv[1]); - int pos = atoi(argv[2]); - - if (motorid < 1 || motorid > 6) { - ZLOGE(TAG, "motorid out of range"); - return; - } - - g_step_motor45[motorid].moveTo(pos); - }); - - cmdScheduler.registerCmd("stepmotor45_moveBy", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(2); - int motorid = atoi(argv[1]); - int pos = atoi(argv[2]); - - if (motorid < 1 || motorid > 6) { - ZLOGE(TAG, "motorid out of range"); - return; - } - - g_step_motor45[motorid].moveBy(pos); - }); - - cmdScheduler.registerCmd("stepmotor45_stop", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(1); - int motorid = atoi(argv[1]); - - if (motorid < 1 || motorid > 6) { - ZLOGE(TAG, "motorid out of range"); - return; - } - - g_step_motor45[motorid].stop(); - }); - - cmdScheduler.registerCmd("stepmotor45_setSpeed", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(1); - int motorid = atoi(argv[1]); - int speed = atoi(argv[2]); - - if (motorid < 1 || motorid > 6) { - ZLOGE(TAG, "motorid out of range"); - return; - } - - g_step_motor45[motorid].setDefaultSpeed(speed); - }); - -/******************************************************************************* - * 步进电机 * - *******************************************************************************/ -#define GET_MOTOR(motor) \ - { \ - int motorid = atoi(argv[1]); \ - motor = &g_step_motor[motorid]; \ - if (motorid >= 6 || motorid < 1) { \ - ZLOGE(TAG, "motor %d not found", motorid); \ - context->breakflag = true; \ - return; \ - } \ - } - - /** - * @brief STEPMOTOR - */ - cmdScheduler.registerCmd("step_motor_setvelocity", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(2); - - StepMotor* motor = NULL; - GET_MOTOR(motor); - - motor->setVelocity(atoi(argv[2])); - }); - cmdScheduler.registerCmd("step_motor_set_acc", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(2); - - StepMotor* motor = NULL; - GET_MOTOR(motor); - - motor->setAcc(atoi(argv[2])); - }); - cmdScheduler.registerCmd("step_motor_set_dec", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(2); - - StepMotor* motor = NULL; - GET_MOTOR(motor); - - motor->setDec(atoi(argv[2])); - }); - cmdScheduler.registerCmd("step_motor_moveto", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(2); - - StepMotor* motor = NULL; - GET_MOTOR(motor); - - motor->moveTo(atoi(argv[2])); - }); - cmdScheduler.registerCmd("step_motor_moveby", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(2); - - StepMotor* motor = NULL; - GET_MOTOR(motor); - - motor->moveBy(atoi(argv[2])); - }); - cmdScheduler.registerCmd("step_motor_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(2); - - StepMotor* motor = NULL; - GET_MOTOR(motor); - - motor->rotate(atoi(argv[2])); - }); - cmdScheduler.registerCmd("step_motor_movetozero", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(1); - - StepMotor* motor = NULL; - GET_MOTOR(motor); - - motor->moveToZero(); - }); - cmdScheduler.registerCmd("step_motor_wait_for_idle", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(1); - - StepMotor* motor = NULL; - GET_MOTOR(motor); - HAL_Delay(100); - while (!motor->isIdleState()) { - HAL_Delay(300); - ZLOGI(TAG, "step_motor_wait_for_idle %d", atoi(argv[1])); - } - }); - cmdScheduler.registerCmd("step_motor_clear_exception", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(1); - - StepMotor* motor = NULL; - GET_MOTOR(motor); - - motor->clearException(); - }); - - cmdScheduler.registerCmd("step_motor_moveto_mm", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(2); - - StepMotor* motor = NULL; - GET_MOTOR(motor); - - int32_t step; - if (!distance_mm_to_step(atoi(argv[1]), atof(argv[2]), &step)) { - ZLOGE(TAG, "step_motor_moveto_mm %d %f failed", atoi(argv[1]), atof(argv[2])); - context->breakflag = true; - return; - } - motor->moveTo(step); - }); - - static int main_servo_motor_velocity = 400; - - cmdScheduler.registerCmd("main_servo_motor_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - // main_servo_motor_rotate direction - CHECK_ARGC(1); - int direction = atoi(argv[1]); - if (direction == 0) { - // 用正转位置0来停止设备 - g_eq20servomotor.writePn(1, 614, 1); - g_eq20servomotor.writePn(1, 1501, 0); - g_eq20servomotor.writePn(1, 1501, 1); - g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity); - - } else if (direction > 0) { - g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity); - g_eq20servomotor.writePn(1, 614, -1); - g_eq20servomotor.writePn(1, 1501, 0); - g_eq20servomotor.writePn(1, 1501, 1); - g_eq20servomotor.writePn(1, 614, 999999); - g_eq20servomotor.writePn(1, 1501, 0); - g_eq20servomotor.writePn(1, 1501, 1); - } else if (direction < 0) { - g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity); - g_eq20servomotor.writePn(1, 614, 1); - g_eq20servomotor.writePn(1, 1501, 0); - g_eq20servomotor.writePn(1, 1501, 1); - g_eq20servomotor.writePn(1, 614, -999999); - g_eq20servomotor.writePn(1, 1501, 0); - g_eq20servomotor.writePn(1, 1501, 1); - } - }); - cmdScheduler.registerCmd("main_servo_motor_set_velocity", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(1); - main_servo_motor_velocity = atoi(argv[1]); - return; - }); - cmdScheduler.registerCmd("main_servo_motor_stop", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - // main_servo_motor_move_to v - CHECK_ARGC(0); - g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity); - g_eq20servomotor.writePn(1, 614, 0); - g_eq20servomotor.writePn(1, 1501, 0); - g_eq20servomotor.writePn(1, 1501, 1); - }); - cmdScheduler.registerCmd("main_servo_motor_stop", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - // main_servo_motor_move_to v - CHECK_ARGC(0); - g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity); - g_eq20servomotor.writePn(1, 614, 0); - g_eq20servomotor.writePn(1, 1501, 0); - g_eq20servomotor.writePn(1, 1501, 1); - }); - // setzero // set4095 // moveto @@ -439,24 +151,32 @@ void regfn() { }); cmdScheduler.registerCmd("mini_servo_move_to", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { CHECK_ARGC(3); - int id = atoi(argv[1]); - int pos = atoi(argv[2]); - int v = atoi(argv[3]); - ZLOGI(TAG, "mini_servo_move_to %d %d %d", id, pos, v); - g_feiteservomotor.moveTo(id, pos, v, 0); - }); + int id = atoi(argv[1]); + int pos = atoi(argv[2]); + int torque = atoi(argv[3]); -#if 0 - - cmdScheduler.registerCmd("main_servo_motor_stop", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - // main_servo_motor_move_to v - CHECK_ARGC(0); - g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity); - g_eq20servomotor.writePn(1, 614, 0); - g_eq20servomotor.writePn(1, 1501, 0); - g_eq20servomotor.writePn(1, 1501, 1); + ZLOGI(TAG, "mini_servo_move_to %d %d %d %d", id, pos, 2700, torque); + g_feiteservomotor.moveTo(id, pos, 2700, torque); + }); + cmdScheduler.registerCmd("mini_servo_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(2); + int id = atoi(argv[1]); + int speed = atoi(argv[2]); + // int v = atoi(3000); + ZLOGI(TAG, "mini_servo_rotate %d %d %d", id, speed, 10, 2700); + g_feiteservomotor.rotate(id, speed, 10); + }); + cmdScheduler.registerCmd("mini_servo_move_with_torque", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(2); + int id = atoi(argv[1]); + int torque = atoi(argv[2]); + // int v = atoi(3000); + if (torque == 0) { + g_feiteservomotor.moveWithTorque(id, 1); + } else { + g_feiteservomotor.moveWithTorque(id, torque); + } }); -#endif cmdScheduler.registerCmd("sleep_ms", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { CHECK_ARGC(1); @@ -468,7 +188,10 @@ extern "C" { extern DMA_HandleTypeDef hdma_usart3_rx; extern DMA_HandleTypeDef hdma_usart3_tx; } -void Main::run() { + +extern "C" {} +extern void step_motor_cmd_reg(); +void Main::run() { /******************************************************************************* * 系统初始化 * *******************************************************************************/ @@ -516,6 +239,7 @@ void Main::run() { g_feiteservomotor.initialize(&huart3, &hdma_usart3_rx, &hdma_usart3_tx); + step_motor_cmd_reg(); regfn(); while (true) { OSDefaultSchduler::getInstance()->loop(); diff --git a/usrc/main.hpp b/usrc/main.hpp index 6667d1d..a632901 100644 --- a/usrc/main.hpp +++ b/usrc/main.hpp @@ -19,6 +19,3 @@ class Main { extern Main gmain; } // namespace iflytop -extern "C" { -void umain(void) { iflytop::gmain.run(); } -} diff --git a/usrc/step_motor_cmd_reg.cpp b/usrc/step_motor_cmd_reg.cpp new file mode 100644 index 0000000..a6f3817 --- /dev/null +++ b/usrc/step_motor_cmd_reg.cpp @@ -0,0 +1,352 @@ +#include +#include + +#include "main.hpp" + +// +#include "feite_servo_motor.hpp" +#include "sdk/os/zos.hpp" +#include "sdk\components\cmdscheduler\cmd_scheduler.hpp" +#include "sdk\components\eq_20_asb_motor\eq20_servomotor.hpp" +#include "sdk\components\iflytop_can_slave_module_master_end\stepmotor.hpp" +#include "sdk\components\iflytop_can_slave_v1\iflytop_can_master.hpp" +#include "sdk\components\step_motor_45\step_motor_45.hpp" +#include "sdk\components\step_motor_45\step_motor_45_scheduler.hpp" + +#define TAG "main" +namespace iflytop { +using namespace std; + +}; + +using namespace iflytop; +#define CHECK_ARGC(n) \ + if (argc != (n + 1)) { \ + ZLOGE(TAG, "argc != %d", n); \ + context->breakflag = true; \ + return; \ + } + +static map screw_lead = { + {1, 10.0}, // + {2, 10.0}, // + {3, 10.0}, // + {4, 10.0}, // + {5, 10.0}, // + {6, 10.0}, // +}; + +/******************************************************************************* + * 代码 * + *******************************************************************************/ + +namespace iflytop { +extern StepMotor45 g_step_motor45[7]; +extern StepMotor g_step_motor[10]; +extern IflytopCanMaster m_IflytopCanMaster; +extern CmdScheduler cmdScheduler; + +extern ModbusBlockHost g_modbusblockhost; +extern Eq20ServoMotor g_eq20servomotor; +extern FeiTeServoMotor g_feiteservomotor; +} // namespace iflytop + +static bool distance_mm_to_step(int motorid, float distance_mm, int32_t* distance) { + if (screw_lead.find(motorid) == screw_lead.end()) { + return false; + } + float lead = screw_lead[motorid]; + *distance = distance_mm / lead * 51200; + return true; +} + +void step_motor_cmd_reg() { + cmdScheduler.registerCmd("help", // + [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { ZLOGI(TAG, "do_help"); }); + cmdScheduler.registerCmd("reset_board", // + [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { NVIC_SystemReset(); }); + cmdScheduler.registerCmd("stepmotor45_wait_to_reach_pos", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + // stepmotor45_rotate motorid direction + CHECK_ARGC(2); + int motorid = atoi(argv[1]); + + if (motorid < 1 || motorid > 6) { + ZLOGE(TAG, "motorid out of range"); + return; + } + while (true) { + if (g_step_motor45[motorid].isReachTargetPos()) { + break; + } + ZLOGI(TAG, "stepmotor45_wait_to_reach_pos %d", motorid); + osDelay(100); + } + }); + cmdScheduler.registerCmd("stepmotor45_reset_pos_all", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + // stepmotor45_rotate motorid direction + CHECK_ARGC(0); + g_step_motor45[1].setPos(0); + g_step_motor45[2].setPos(0); + g_step_motor45[3].setPos(0); + g_step_motor45[4].setPos(0); + g_step_motor45[5].setPos(0); + g_step_motor45[6].setPos(0); + }); + + // stepmotor45 + cmdScheduler.registerCmd("stepmotor45_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + // stepmotor45_rotate motorid direction + CHECK_ARGC(2); + int motorid = atoi(argv[1]); + int direction = atoi(argv[2]); + + if (motorid < 1 || motorid > 6) { + ZLOGE(TAG, "motorid out of range"); + return; + } + + g_step_motor45[motorid].rotate(direction); + }); + cmdScheduler.registerCmd("stepmotor45_readPos", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + // stepmotor45_rotate motorid direction + CHECK_ARGC(2); + int motorid = atoi(argv[1]); + + if (motorid < 1 || motorid > 6) { + ZLOGE(TAG, "motorid out of range"); + return; + } + + ZLOGI(TAG, "motorid %d pos %d", motorid, g_step_motor45[motorid].getPos()); + }); + + cmdScheduler.registerCmd("stepmotor45_moveTo", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(2); + int motorid = atoi(argv[1]); + int pos = atoi(argv[2]); + + if (motorid < 1 || motorid > 6) { + ZLOGE(TAG, "motorid out of range"); + return; + } + + g_step_motor45[motorid].moveTo(pos); + }); + + cmdScheduler.registerCmd("stepmotor45_moveBy", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(2); + int motorid = atoi(argv[1]); + int pos = atoi(argv[2]); + + if (motorid < 1 || motorid > 6) { + ZLOGE(TAG, "motorid out of range"); + return; + } + + g_step_motor45[motorid].moveBy(pos); + }); + + cmdScheduler.registerCmd("stepmotor45_stop", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(1); + int motorid = atoi(argv[1]); + + if (motorid < 1 || motorid > 6) { + ZLOGE(TAG, "motorid out of range"); + return; + } + + g_step_motor45[motorid].stop(); + }); + + cmdScheduler.registerCmd("stepmotor45_setSpeed", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(1); + int motorid = atoi(argv[1]); + int speed = atoi(argv[2]); + + if (motorid < 1 || motorid > 6) { + ZLOGE(TAG, "motorid out of range"); + return; + } + + g_step_motor45[motorid].setDefaultSpeed(speed); + }); + +/******************************************************************************* + * 步进电机 * + *******************************************************************************/ +#define GET_MOTOR(motor) \ + { \ + int motorid = atoi(argv[1]); \ + motor = &g_step_motor[motorid]; \ + if (motorid >= 6 || motorid < 1) { \ + ZLOGE(TAG, "motor %d not found", motorid); \ + context->breakflag = true; \ + return; \ + } \ + } + + /** + * @brief STEPMOTOR + */ + cmdScheduler.registerCmd("step_motor_setvelocity", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(2); + + StepMotor* motor = NULL; + GET_MOTOR(motor); + + motor->setVelocity(atoi(argv[2])); + }); + cmdScheduler.registerCmd("step_motor_set_acc", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(2); + + StepMotor* motor = NULL; + GET_MOTOR(motor); + + motor->setAcc(atoi(argv[2])); + }); + cmdScheduler.registerCmd("step_motor_set_dec", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(2); + + StepMotor* motor = NULL; + GET_MOTOR(motor); + + motor->setDec(atoi(argv[2])); + }); + cmdScheduler.registerCmd("step_motor_moveto", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(2); + + StepMotor* motor = NULL; + GET_MOTOR(motor); + + motor->moveTo(atoi(argv[2])); + }); + cmdScheduler.registerCmd("step_motor_moveby", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(2); + + StepMotor* motor = NULL; + GET_MOTOR(motor); + + motor->moveBy(atoi(argv[2])); + }); + cmdScheduler.registerCmd("step_motor_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(2); + + StepMotor* motor = NULL; + GET_MOTOR(motor); + + motor->rotate(atoi(argv[2])); + }); + cmdScheduler.registerCmd("step_motor_movetozero", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(1); + + StepMotor* motor = NULL; + GET_MOTOR(motor); + + motor->moveToZero(); + }); + + cmdScheduler.registerCmd("step_motor_readpos", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(1); + + StepMotor* motor = NULL; + GET_MOTOR(motor); + + int32_t pos = motor->readpos(); + ZLOGI(TAG, "step_motor_readpos %d %d", atoi(argv[1]), pos); + }); + cmdScheduler.registerCmd("step_motor_wait_for_idle", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(1); + + StepMotor* motor = NULL; + GET_MOTOR(motor); + HAL_Delay(100); + while (!motor->isIdleState()) { + HAL_Delay(300); + ZLOGI(TAG, "step_motor_wait_for_idle %d", atoi(argv[1])); + } + }); + cmdScheduler.registerCmd("step_motor_clear_exception", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(1); + + StepMotor* motor = NULL; + GET_MOTOR(motor); + + motor->clearException(); + }); + + cmdScheduler.registerCmd("step_motor_moveto_mm", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(2); + + StepMotor* motor = NULL; + GET_MOTOR(motor); + + int32_t step; + if (!distance_mm_to_step(atoi(argv[1]), atof(argv[2]), &step)) { + ZLOGE(TAG, "step_motor_moveto_mm %d %f failed", atoi(argv[1]), atof(argv[2])); + context->breakflag = true; + return; + } + motor->moveTo(step); + }); + + static int main_servo_motor_velocity = 400; + cmdScheduler.registerCmd("main_servo_motor_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + // main_servo_motor_rotate direction + CHECK_ARGC(1); + int direction = atoi(argv[1]); + if (direction == 0) { + // 用正转位置0来停止设备 + g_eq20servomotor.writePn(1, 614, 1); + g_eq20servomotor.writePn(1, 1501, 0); + g_eq20servomotor.writePn(1, 1501, 1); + g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity); + + } else if (direction > 0) { + g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity); + g_eq20servomotor.writePn(1, 614, -1); + g_eq20servomotor.writePn(1, 1501, 0); + g_eq20servomotor.writePn(1, 1501, 1); + g_eq20servomotor.writePn(1, 614, 999999); + g_eq20servomotor.writePn(1, 1501, 0); + g_eq20servomotor.writePn(1, 1501, 1); + } else if (direction < 0) { + g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity); + g_eq20servomotor.writePn(1, 614, 1); + g_eq20servomotor.writePn(1, 1501, 0); + g_eq20servomotor.writePn(1, 1501, 1); + g_eq20servomotor.writePn(1, 614, -999999); + g_eq20servomotor.writePn(1, 1501, 0); + g_eq20servomotor.writePn(1, 1501, 1); + } + }); + cmdScheduler.registerCmd("main_servo_motor_set_velocity", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(1); + main_servo_motor_velocity = atoi(argv[1]); + return; + }); + cmdScheduler.registerCmd("main_servo_motor_stop", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + // main_servo_motor_move_to v + CHECK_ARGC(0); + g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity); + g_eq20servomotor.writePn(1, 614, 0); + g_eq20servomotor.writePn(1, 1501, 0); + g_eq20servomotor.writePn(1, 1501, 1); + }); + cmdScheduler.registerCmd("main_servo_motor_stop", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + // main_servo_motor_move_to v + CHECK_ARGC(0); + g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity); + g_eq20servomotor.writePn(1, 614, 0); + g_eq20servomotor.writePn(1, 1501, 0); + g_eq20servomotor.writePn(1, 1501, 1); + }); + + // setzero + // set4095 + // moveto + + cmdScheduler.registerCmd("sleep_ms", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(1); + osDelay(atoi(argv[1])); + }); +}