You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
284 lines
12 KiB
284 lines
12 KiB
#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"
|
|
#include "sdk\components\zcancmder\zcanreceiver_master.hpp"
|
|
//
|
|
#include "sdk\components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.hpp"
|
|
#include "sdk\components/xy_robot_ctrl_module/xy_robot_script_cmder_module.hpp"
|
|
#include "sdk\components\cmdscheduler\cmd_scheduler_v2.hpp"
|
|
#include "sdk\components\eq_20_asb_motor\script_cmder_eq20_servomotor.hpp"
|
|
#include "sdk\components\mini_servo_motor\mini_servo_motor_ctrl_module.hpp"
|
|
#include "sdk\components\mini_servo_motor\scirpt_cmder_mini_servo_motor_ctrl_module.hpp"
|
|
#include "sdk\components\step_motor_45\script_cmder_step_motor_45.hpp"
|
|
// #include "sdk\components\scriptcmder_module\xy_robot_script_cmder_module.hpp"
|
|
// #include "sdk\components\zcancmder_master_module/zcan_master_step_motor_ctrl_module.hpp"
|
|
// #include "sdk\components\zcancmder_master_module\zcan_xy_robot_master_module.hpp"
|
|
|
|
#include "intelligent_winding_robot_ctrl.hpp"
|
|
#include "sdk\components\taojingchi_screen\taojingchi_screen_service.hpp"
|
|
#include "sdk\components\zprotocol_helper\micro_computer_module_device_script_cmder_paser.hpp"
|
|
#include "sdk\components\zprotocols\zcancmder_v2\protocol_proxy.hpp"
|
|
#include "sdk\components\zprotocols\zcancmder_v2\zmodule_device_manager.hpp"
|
|
#include "sdk\components\zprotocols\zcancmder_v2\zmodule_device_script_cmder_paser.hpp"
|
|
namespace iflytop {
|
|
extern Main gmain;
|
|
};
|
|
|
|
using namespace iflytop;
|
|
using namespace std;
|
|
#define TAG "CMD"
|
|
extern "C" {}
|
|
/*******************************************************************************
|
|
* 配置 *
|
|
*******************************************************************************/
|
|
#if 0
|
|
static StepMotor45::cfg_t cfg4 = {
|
|
.max_pos = -1,
|
|
.enable_zero_limit = false,
|
|
.enable_max_pos_limit = false,
|
|
.mirror = true,
|
|
|
|
.zeroPin = PinNull,
|
|
.zeroPinMirror = false,
|
|
|
|
.driverPin = {PE0, PE2, PE4, PE6},
|
|
.driverPinMirror = true,
|
|
};
|
|
|
|
static StepMotor45::cfg_t cfg5 = {
|
|
.max_pos = -1,
|
|
.enable_zero_limit = false,
|
|
.enable_max_pos_limit = false,
|
|
.mirror = true,
|
|
|
|
.zeroPin = PinNull,
|
|
.zeroPinMirror = false,
|
|
|
|
.driverPin = {PC13, PE5, PE3, PE1},
|
|
.driverPinMirror = true,
|
|
};
|
|
static StepMotor45::cfg_t cfg6 = {
|
|
.max_pos = -1,
|
|
.enable_zero_limit = false,
|
|
.enable_max_pos_limit = false,
|
|
.mirror = true,
|
|
|
|
.zeroPin = PinNull,
|
|
.zeroPinMirror = false,
|
|
|
|
.driverPin = {PC12, PD3, PD5, PD7},
|
|
.driverPinMirror = true,
|
|
};
|
|
#endif
|
|
|
|
namespace iflytop {
|
|
/*******************************************************************************
|
|
* 基础组件 *
|
|
*******************************************************************************/
|
|
extern ZCanCommnaderMaster m_zcanCommnaderMaster; // can总线
|
|
extern ModbusBlockHost g_modbusblockhost; // modbus总线
|
|
extern FeiTeServoMotor g_feiteservomotor_bus; // 飞特舵机总线
|
|
extern ZModuleDeviceManager g_zmodule_device_manager; // 用于管理所有的设备
|
|
extern StepMotor45Scheduler step_motor45_scheduler; // 45步进电机调度器
|
|
|
|
extern CmdSchedulerV2 g_cmdScheduler; // 串口字符串指令总线
|
|
extern TaoJingChiScreenService g_taojingchi_screen_service; // 淘精驰屏幕服务
|
|
extern MicroComputerModuleDeviceScriptCmderPaser g_zmodule_device_script_cmder_paser; // 用于解析所有的设备指令
|
|
|
|
/*******************************************************************************
|
|
* 本地设备 *
|
|
*******************************************************************************/
|
|
extern Eq20ServoMotor g_main_servo_motor;
|
|
extern StepMotor45 g_step_motor45[7];
|
|
extern MiniRobotCtrlModule g_mini_servo[6];
|
|
/*******************************************************************************
|
|
* CAN设备 *
|
|
*******************************************************************************/
|
|
extern ZIProtocolProxy g_xyrobotctrlmodule;
|
|
extern ZIProtocolProxy g_z_step_motor;
|
|
|
|
extern IntelligentWindingRobotCtrl g_intelligent_winding_robot_ctrl;
|
|
|
|
} // namespace iflytop
|
|
|
|
extern "C" {
|
|
extern DMA_HandleTypeDef hdma_usart3_rx;
|
|
extern DMA_HandleTypeDef hdma_usart3_tx;
|
|
}
|
|
|
|
extern "C" {}
|
|
extern DMA_HandleTypeDef hdma_usart2_rx;
|
|
extern DMA_HandleTypeDef hdma_usart2_tx;
|
|
|
|
void script_reg_fn() {
|
|
#if 0
|
|
xy_robot_ctrl_enable (id,en)
|
|
xy_robot_ctrl_factory_reset (id)
|
|
xy_robot_ctrl_flush (id)
|
|
xy_robot_ctrl_force_change_current_pos (id,x,y)
|
|
xy_robot_ctrl_get_base_param (id)
|
|
xy_robot_ctrl_move_by (id,dx,dy,v)
|
|
|
|
|
|
xy_robot_ctrl_move_to (id,x,y,speed)
|
|
xy_robot_ctrl_move_to_zero (id)
|
|
xy_robot_ctrl_move_to_zero_with_calibrate (id,nowx,nowy)
|
|
xy_robot_ctrl_stop (id,stop_type)
|
|
#endif
|
|
#define XY_ROBOT_ID 3
|
|
|
|
g_cmdScheduler.regCMD("xy_robot_ctrl_enable", "(id,en)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
int32_t en = atoi(paraV[1]);
|
|
int32_t ecode = g_zmodule_device_manager.xymotor_enable(XY_ROBOT_ID, en);
|
|
ack->setNoneAck(ecode);
|
|
});
|
|
|
|
g_cmdScheduler.regCMD("xy_robot_ctrl_factory_reset", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
int32_t ecode = g_zmodule_device_manager.module_factory_reset(XY_ROBOT_ID);
|
|
ack->setNoneAck(ecode);
|
|
});
|
|
|
|
g_cmdScheduler.regCMD("xy_robot_ctrl_flush", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
int32_t ecode = g_zmodule_device_manager.module_flush_cfg(XY_ROBOT_ID);
|
|
ack->setNoneAck(ecode);
|
|
});
|
|
#if 0
|
|
g_cmdScheduler.regCMD("xy_robot_ctrl_force_change_current_pos", "(id,x,y)", 3, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
int32_t x = atoi(paraV[1]);
|
|
int32_t y = atoi(paraV[2]);
|
|
int32_t ecode = g_zmodule_device_manager.motor_set_current_pos_by_change_shift(XY_ROBOT_ID, x, y);
|
|
ack->setNoneAck(ecode);
|
|
});
|
|
#endif
|
|
|
|
g_cmdScheduler.regCMD("xy_robot_ctrl_get_base_param", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) { g_zmodule_device_script_cmder_paser.do_dumpconfig(paramN, paraV, ack); });
|
|
|
|
g_cmdScheduler.regCMD("xy_robot_ctrl_move_by", "(id,dx,dy,v)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
int32_t dx = atoi(paraV[1]);
|
|
int32_t dy = atoi(paraV[2]);
|
|
int32_t v = atoi(paraV[3]);
|
|
int32_t ecode = g_zmodule_device_manager.xymotor_move_by(XY_ROBOT_ID, dx, dy, v);
|
|
ack->setNoneAck(ecode);
|
|
});
|
|
|
|
g_cmdScheduler.regCMD("xy_robot_ctrl_move_to", "(id,x,y,speed)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
int32_t x = atoi(paraV[1]);
|
|
int32_t y = atoi(paraV[2]);
|
|
int32_t speed = atoi(paraV[3]);
|
|
int32_t ecode = g_zmodule_device_manager.xymotor_move_to(XY_ROBOT_ID, x, y, speed);
|
|
ack->setNoneAck(ecode);
|
|
});
|
|
|
|
g_cmdScheduler.regCMD("xy_robot_ctrl_move_to_zero", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
int32_t ecode = g_zmodule_device_manager.xymotor_move_to_zero(XY_ROBOT_ID);
|
|
ack->setNoneAck(ecode);
|
|
});
|
|
|
|
g_cmdScheduler.regCMD("xy_robot_ctrl_move_to_zero_with_calibrate", "(id,nowx,nowy)", 3, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
int32_t nowx = atoi(paraV[1]);
|
|
int32_t nowy = atoi(paraV[2]);
|
|
int32_t ecode = g_zmodule_device_manager.xymotor_move_to_zero_and_calculated_shift(XY_ROBOT_ID);
|
|
ack->setNoneAck(ecode);
|
|
});
|
|
|
|
g_cmdScheduler.regCMD("xy_robot_ctrl_stop", "(id,stop_type)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
int32_t stop_type = atoi(paraV[1]);
|
|
int32_t ecode = g_zmodule_device_manager.module_stop(XY_ROBOT_ID);
|
|
ack->setNoneAck(ecode);
|
|
});
|
|
#if 0
|
|
00102227 INFO [CMD ] step_motor_ctrl_enable (id,en)
|
|
00102251 INFO [CMD ] step_motor_ctrl_get_base_param (id)
|
|
00102277 INFO [CMD ] step_motor_ctrl_move_by (id,dx,speed)
|
|
00102283 INFO [CMD ] step_motor_ctrl_move_to (id,x,speed)
|
|
00102298 INFO [CMD ] step_motor_ctrl_move_to_zero (id)
|
|
00102304 INFO [CMD ] step_motor_ctrl_move_to_zero_with_calibrate (id,x)
|
|
00102311 INFO [CMD ] step_motor_ctrl_read_detailed_status (id)
|
|
00102318 INFO [CMD ] step_motor_ctrl_read_status (id)
|
|
00102330 INFO [CMD ] step_motor_ctrl_rotate (id,speed,lastforms)
|
|
00102362 INFO [CMD ] step_motor_ctrl_stop (id,stop_type)
|
|
#endif
|
|
|
|
g_cmdScheduler.regCMD("step_motor_ctrl_enable", "(id,en)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
int32_t en = atoi(paraV[1]);
|
|
int32_t ecode = g_zmodule_device_manager.motor_enable(4, en);
|
|
ack->setNoneAck(ecode);
|
|
});
|
|
|
|
g_cmdScheduler.regCMD("step_motor_ctrl_get_base_param", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) { //
|
|
g_zmodule_device_script_cmder_paser.do_dumpconfig(4);
|
|
});
|
|
|
|
g_cmdScheduler.regCMD("step_motor_ctrl_move_by", "(id,dx,speed)", 3, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
int32_t dx = atoi(paraV[1]);
|
|
int32_t speed = atoi(paraV[2]);
|
|
int32_t ecode = g_zmodule_device_manager.motor_move_by(4, dx, speed, 0);
|
|
ack->setNoneAck(ecode);
|
|
});
|
|
|
|
g_cmdScheduler.regCMD("step_motor_ctrl_move_to", "(id,x,speed)", 3, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
int32_t x = atoi(paraV[1]);
|
|
int32_t speed = atoi(paraV[2]);
|
|
int32_t ecode = g_zmodule_device_manager.motor_move_to(4, x, speed, 0);
|
|
ack->setNoneAck(ecode);
|
|
});
|
|
|
|
g_cmdScheduler.regCMD("step_motor_ctrl_move_to_zero", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
int32_t ecode = g_zmodule_device_manager.motor_move_to_zero_backward(4, 450, 300, 2000, 0);
|
|
ack->setNoneAck(ecode);
|
|
});
|
|
|
|
g_cmdScheduler.regCMD("step_motor_ctrl_move_to_zero_with_calibrate", "(id,x)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
int32_t x = atoi(paraV[1]);
|
|
int32_t ecode = g_zmodule_device_manager.motor_move_to_zero_backward_and_calculated_shift(4, 0, 0, 0, 0);
|
|
ack->setNoneAck(ecode);
|
|
});
|
|
|
|
g_cmdScheduler.regCMD("step_motor_ctrl_read_detailed_status", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
// g_zmodule_device_script_cmder_paser.motor_read_pos(4);
|
|
});
|
|
|
|
g_cmdScheduler.regCMD("step_motor_ctrl_read_status", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
// g_zmodule_device_script_cmder_paser.motor_read_pos(4);
|
|
|
|
//
|
|
|
|
int32_t pos;
|
|
int32_t errocode;
|
|
int32_t status;
|
|
|
|
g_zmodule_device_manager.motor_read_pos(4, &pos);
|
|
g_zmodule_device_manager.module_get_error(4, &errocode);
|
|
g_zmodule_device_manager.module_get_status(4, &status);
|
|
|
|
ZLOGI(TAG, "pos:%d,errocode:%d,status:%d", pos, errocode, status);
|
|
|
|
|
|
});
|
|
|
|
// g_cmdScheduler.regCMD("step_motor_ctrl_rotate", "(id,speed,lastforms)", 3, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
// int32_t speed = atoi(paraV[1]);
|
|
// int32_t lastforms = atoi(paraV[2]);
|
|
// int32_t direction = speed > 0 ? 1 : -1;
|
|
// int32_t ecode = g_zmodule_device_manager.motor_rotate(4,direction ,abs(speed), lastforms);
|
|
// ack->setNoneAck(ecode);
|
|
// });
|
|
|
|
g_cmdScheduler.regCMD("step_motor_ctrl_stop", "(id,stop_type)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
|
|
int32_t stop_type = atoi(paraV[1]);
|
|
int32_t ecode = g_zmodule_device_manager.module_stop(4);
|
|
ack->setNoneAck(ecode);
|
|
});
|
|
}
|