zuniversal_hardware_driver
/
iflytop_cancmder
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update

master
zhaohe 2 years ago
parent
9aaf45c0bf
commit
6b5501be0a
5 changed files with 392 additions and 317 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      sdk
  2. 4
      usrc/feite_servo_motor.cpp
  3. 348
      usrc/main.cpp
  4. 3
      usrc/main.hpp
  5. 352
      usrc/step_motor_cmd_reg.cpp

2
sdk

@ -1 +1 @@
Subproject commit 3307f7a4c801857b8cd9cab5abb290f88ffc3155
Subproject commit 2b35686d8d692c80c80bdbb6b7e4afa38d94f860

4
usrc/feite_servo_motor.cpp
View File

@ -17,11 +17,13 @@ using namespace feite;
} }
static void dumphex(const char* tag, uint8_t* data, uint8_t len) { static void dumphex(const char* tag, uint8_t* data, uint8_t len) {
#if 1
printf("%s:", tag); printf("%s:", tag);
for (int i = 0; i < len; i++) { for (int i = 0; i < len; i++) {
printf("%02x ", data[i]); printf("%02x ", data[i]);
} }
printf("\n"); printf("\n");
#endif
} }
void FeiTeServoMotor::initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx) { void FeiTeServoMotor::initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx) {
m_uart = uart; 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); return write_u16(id, add, val);
} }
bool FeiTeServoMotor::write_reg(uint8_t id, bool async, uint8_t add, uint8_t* data, uint8_t len) { // 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; cmd_header_t* cmd_header = (cmd_header_t*)m_txbuf;
receipt_header_t* receipt_header = (receipt_header_t*)m_rxbuf; receipt_header_t* receipt_header = (receipt_header_t*)m_rxbuf;
cmd_header->header = 0xffff; cmd_header->header = 0xffff;

348
usrc/main.cpp
View File

@ -27,7 +27,7 @@ using namespace std;
return; \ return; \
} }
extern "C" { 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, .driverPinMirror = true,
}; };
map<int, float> 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; ModbusBlockHost g_modbusblockhost;
Eq20ServoMotor g_eq20servomotor; Eq20ServoMotor g_eq20servomotor;
FeiTeServoMotor g_feiteservomotor; 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() { 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 // setzero
// set4095 // set4095
// moveto // moveto
@ -439,24 +151,32 @@ void regfn() {
}); });
cmdScheduler.registerCmd("mini_servo_move_to", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { cmdScheduler.registerCmd("mini_servo_move_to", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
CHECK_ARGC(3); 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) { cmdScheduler.registerCmd("sleep_ms", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
CHECK_ARGC(1); CHECK_ARGC(1);
@ -468,7 +188,10 @@ extern "C" {
extern DMA_HandleTypeDef hdma_usart3_rx; extern DMA_HandleTypeDef hdma_usart3_rx;
extern DMA_HandleTypeDef hdma_usart3_tx; 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); g_feiteservomotor.initialize(&huart3, &hdma_usart3_rx, &hdma_usart3_tx);
step_motor_cmd_reg();
regfn(); regfn();
while (true) { while (true) {
OSDefaultSchduler::getInstance()->loop(); OSDefaultSchduler::getInstance()->loop();

3
usrc/main.hpp
View File

@ -19,6 +19,3 @@ class Main {
extern Main gmain; extern Main gmain;
} // namespace iflytop } // namespace iflytop
extern "C" {
void umain(void) { iflytop::gmain.run(); }
}

352
usrc/step_motor_cmd_reg.cpp
View File

@ -0,0 +1,352 @@
#include <stddef.h>
#include <stdio.h>
#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<int, float> 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]));
});
}
Write Preview
Loading…
Cancel
Save