diff --git a/usrc/main.cpp b/usrc/main.cpp index 3fd88fa..ee65cbb 100644 --- a/usrc/main.cpp +++ b/usrc/main.cpp @@ -6,6 +6,8 @@ // #include "sdk/os/zos.hpp" #include "sdk\components\cmdscheduler\cmd_scheduler.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" @@ -16,7 +18,12 @@ Main gmain; using namespace iflytop; using namespace std; - +#define CHECK_ARGC(n) \ + if (argc != (n + 1)) { \ + ZLOGE(TAG, "argc != %d", n); \ + context->breakflag = true; \ + return; \ + } extern "C" { void StartDefaultTask(void const* argument) { umain(); } } @@ -94,16 +101,226 @@ static StepMotor45::cfg_t cfg5 = { .driverPin = {PA13, PA14, PA15, PC10}, .driverPinMirror = false, }; +static StepMotor45::cfg_t cfg6 = { + .max_pos = -1, + .enable_zero_limit = false, + .enable_max_pos_limit = false, + .mirror = false, + + .zeroPin = PinNull, + .zeroPinMirror = false, + + .driverPin = {PC12, PD3, PD5, PD7}, + .driverPinMirror = false, +}; + +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_1; -static StepMotor45 g_step_motor45_2; -static StepMotor45 g_step_motor45_3; -static StepMotor45 g_step_motor45_4; -static StepMotor45 g_step_motor45_5; +static StepMotor45 g_step_motor45[7]; +StepMotor g_step_motor[10]; +IflytopCanMaster m_IflytopCanMaster; +static CmdScheduler cmdScheduler; + +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 regfn() { + cmdScheduler.registerCmd("help", // + [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { ZLOGI(TAG, "do_help"); }); + + // stepmotor45 + cmdScheduler.registerCmd("stepmotor45_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + // stepmotor45_rotate motorid direction + CHECK_ARGC(2); + int motorid = atoi(argv[1]); + bool 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) { + // stepmotor45_rotate motorid pos + 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) { + // stepmotor45_rotate motorid pos + 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) { + // stepmotor45_rotate motorid pos + 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(); + }); + +/******************************************************************************* + * 步进电机 * + *******************************************************************************/ +#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); + }); +} void Main::run() { /******************************************************************************* @@ -115,31 +332,44 @@ void Main::run() { zos_cfg_t zoscfg; zos_init(&zoscfg); - g_step_motor45_1.initialize(cfg1); - g_step_motor45_2.initialize(cfg2); - g_step_motor45_3.initialize(cfg3); - g_step_motor45_4.initialize(cfg4); - g_step_motor45_5.initialize(cfg5); + auto config = m_IflytopCanMaster.createDefaultConfig(1); + m_IflytopCanMaster.initialize(config); + + int i = 1; + g_step_motor[i++].initialize(11, 10000, &m_IflytopCanMaster); + g_step_motor[i++].initialize(12, 10000, &m_IflytopCanMaster); + g_step_motor[i++].initialize(13, 10000, &m_IflytopCanMaster); + g_step_motor[i++].initialize(14, 10000, &m_IflytopCanMaster); + g_step_motor[i++].initialize(15, 10000, &m_IflytopCanMaster); + g_step_motor[i++].initialize(16, 10000, &m_IflytopCanMaster); + + // g_step_motor45[0].initialize(cfg1); + g_step_motor45[1].initialize(cfg1); + g_step_motor45[2].initialize(cfg2); + g_step_motor45[3].initialize(cfg3); + g_step_motor45[4].initialize(cfg4); + g_step_motor45[5].initialize(cfg5); + g_step_motor45[6].initialize(cfg6); StepMotor45Scheduler step_motor45_scheduler; step_motor45_scheduler.initialize(&htim10, 1000); - step_motor45_scheduler.addMotor(&g_step_motor45_1); - step_motor45_scheduler.addMotor(&g_step_motor45_2); - step_motor45_scheduler.addMotor(&g_step_motor45_3); - step_motor45_scheduler.addMotor(&g_step_motor45_4); - step_motor45_scheduler.addMotor(&g_step_motor45_5); + step_motor45_scheduler.addMotor(&g_step_motor45[1]); + step_motor45_scheduler.addMotor(&g_step_motor45[2]); + step_motor45_scheduler.addMotor(&g_step_motor45[3]); + step_motor45_scheduler.addMotor(&g_step_motor45[4]); + step_motor45_scheduler.addMotor(&g_step_motor45[5]); + step_motor45_scheduler.addMotor(&g_step_motor45[6]); // g_step_motor45_1.rotate(true, 1000); step_motor45_scheduler.start(); - CmdScheduler cmdScheduler; cmdScheduler.initialize(&DEBUG_UART, 1000); - cmdScheduler.registerCmd("help", // - [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { ZLOGI(TAG, "do_help"); }); - + regfn(); while (true) { OSDefaultSchduler::getInstance()->loop(); cmdScheduler.schedule(); + m_IflytopCanMaster.periodicJob(); + osDelay(1); } }