iflytop_cancmder/usrc/main.cpp

#include "main.hpp"

#include <stddef.h>
#include <stdio.h>

//
#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"
#define TAG "main"
namespace iflytop {
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) { iflytop::gmain.run(); }
}
/*******************************************************************************
 *                                    ����                                     *
 *******************************************************************************/
static chip_cfg_t chipcfg = {
    .us_dleay_tim          = &DELAY_US_TIMER,
    .tim_irq_scheduler_tim = &TIM_IRQ_SCHEDULER_TIMER,
    .huart                 = &DEBUG_UART,
    .debuglight            = DEBUG_LIGHT_GPIO,
};

static StepMotor45::cfg_t cfg1 = {
    .max_pos              = -1,
    .enable_zero_limit    = true,
    .enable_max_pos_limit = false,
    .mirror               = true,

    .zeroPin       = PB13,
    .ioPollType    = ZGPIO::kMode_pullup,
    .zeroPinMirror = true,

    .driverPin       = {PB15, PD11, PD12, PD13},
    .driverPinMirror = true,
};

static StepMotor45::cfg_t cfg2 = {
    .max_pos              = -1,
    .enable_zero_limit    = true,
    .enable_max_pos_limit = false,
    .mirror               = true,

    .zeroPin       = PG1,
    .ioPollType    = ZGPIO::kMode_pullup,
    .zeroPinMirror = true,

    .driverPin       = {PG2, PG3, PG4, PG5},
    .driverPinMirror = true,
};

static StepMotor45::cfg_t cfg3 = {
    .max_pos              = -1,
    .enable_zero_limit    = true,
    .enable_max_pos_limit = false,
    .mirror               = true,

    .zeroPin       = PB12,
    .ioPollType    = ZGPIO::kMode_pullup,
    .zeroPinMirror = true,

    .driverPin       = {PG6, PG7, PG8, PC6},
    .driverPinMirror = true,
};
#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 {
/*******************************************************************************
 *                                  ��������                                   *
 *******************************************************************************/
ZCanCommnaderMaster  m_zcanCommnaderMaster;     // can����
ModbusBlockHost      g_modbusblockhost;         // modbus����
FeiTeServoMotor      g_feiteservomotor_bus;     // ���ض�������
ZModuleDeviceManager g_zmodule_device_manager;  // ���ڹ������е��豸
StepMotor45Scheduler step_motor45_scheduler;    // 45��������������

CmdSchedulerV2                            g_cmdScheduler;                       // �����ַ���ָ������
TaoJingChiScreenService                   g_taojingchi_screen_service;          // �Ծ�����Ļ����
MicroComputerModuleDeviceScriptCmderPaser g_zmodule_device_script_cmder_paser;  // ���ڽ������е��豸ָ��

/*******************************************************************************
 *                                  �����豸                                   *
 *******************************************************************************/
Eq20ServoMotor      g_main_servo_motor;
StepMotor45         g_step_motor45[7];
MiniRobotCtrlModule g_mini_servo[6];
/*******************************************************************************
 *                                   CAN�豸                                   *
 *******************************************************************************/
ZIProtocolProxy g_xyrobotctrlmodule;
ZIProtocolProxy g_z_step_motor;

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 regfn() {
#if 0
    eq20_enable (id,en)
    eq20_get_io_state (id)
    eq20_get_pos (id)
    eq20_get_servo_internal_state (id)
    eq20_move_by (id,pos,rpm,acctime)
    eq20_move_to (id,pos,rpm,acctime)
    eq20_move_to_zero_backward (id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)
    eq20_move_to_zero_forward (id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)
    eq20_rotate (id,rpm,acctime)
    eq20_stop (id)

    mini_servo_enable (id,en)
    mini_servo_move_backward (id,torque)
    mini_servo_move_by (id,pos,speed,torque)
    mini_servo_move_forward (id,torque)
    mini_servo_move_to (id,pos,speed,torque)
    mini_servo_position_calibrate (id,pos)
    mini_servo_read_status (id)
    mini_servo_rotate (id,speed,torque,time)
    mini_servo_stop (id,stop_type)
#endif
  g_cmdScheduler.regCMD("eq20_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(2, en);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("eq20_get_io_state", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t en      = atoi(paraV[1]);
    int32_t iostate = 0;
    int32_t ecode   = g_zmodule_device_manager.module_readio(2, &iostate);
    ack->setInt32Ack(ecode, iostate);
  });

  g_cmdScheduler.regCMD("eq20_get_pos", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t pos   = 0;
    int32_t ecode = g_zmodule_device_manager.motor_read_pos(2, &pos);
    ack->setInt32Ack(ecode, pos);
  });

  g_cmdScheduler.regCMD("eq20_get_servo_internal_state", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t pos   = 0;
    int32_t ecode = g_zmodule_device_manager.motor_read_pos(2, &pos);
    ack->setInt32Ack(ecode, pos);
  });

  g_cmdScheduler.regCMD("eq20_move_by", "(id,pos,rpm,acctime)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t pos     = atoi(paraV[1]);
    int32_t rpm     = atoi(paraV[2]);
    int32_t acctime = atoi(paraV[3]);
    int32_t ecode   = g_zmodule_device_manager.motor_move_by_acctime(2, pos, rpm, acctime);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("eq20_move_to", "(id,pos,rpm,acctime)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t pos     = atoi(paraV[1]);
    int32_t rpm     = atoi(paraV[2]);
    int32_t acctime = atoi(paraV[3]);
    int32_t ecode   = g_zmodule_device_manager.motor_move_to_acctime(2, pos, rpm, acctime);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("eq20_move_to_zero_backward", "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t lookzeropoint_rpm      = atoi(paraV[1]);
    int32_t findzero_edge_rpm      = atoi(paraV[2]);
    int32_t lookzeropoint_acc_time = atoi(paraV[3]);
    int32_t ecode                  = g_zmodule_device_manager.motor_move_to_zero_backward(2, lookzeropoint_rpm, findzero_edge_rpm, 0, 0);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("eq20_move_to_zero_forward", "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t lookzeropoint_rpm      = atoi(paraV[1]);
    int32_t findzero_edge_rpm      = atoi(paraV[2]);
    int32_t lookzeropoint_acc_time = atoi(paraV[3]);
    int32_t ecode                  = g_zmodule_device_manager.motor_move_to_zero_forward(2, lookzeropoint_rpm, findzero_edge_rpm, 0, 0);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("eq20_rotate", "(id,rpm,acctime)", 3, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t rpm     = atoi(paraV[1]);
    int32_t acctime = atoi(paraV[2]);

    int32_t absrpm    = abs(rpm);
    int32_t direction = rpm > 0 ? 1 : -1;

    int32_t ecode = g_zmodule_device_manager.motor_rotate_acctime(2, direction, absrpm, acctime);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("eq20_stop", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t ecode = g_zmodule_device_manager.module_stop(2);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("mini_servo_enable", "(id,en)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t id    = atoi(paraV[0]);
    int32_t en    = atoi(paraV[1]);
    int32_t ecode = g_zmodule_device_manager.motor_enable(id + 10, en);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("mini_servo_move_backward", "(id,torque)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t id     = atoi(paraV[0]);
    int32_t torque = atoi(paraV[1]);
    torque         = abs(torque);
    int32_t ecode  = g_zmodule_device_manager.motor_move_to_with_torque(id + 10, 0, torque);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("mini_servo_move_by", "(id,pos,speed,torque)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t id     = atoi(paraV[0]);
    int32_t pos    = atoi(paraV[1]);
    int32_t speed  = atoi(paraV[2]);
    int32_t torque = atoi(paraV[3]);
    int32_t ecode  = g_zmodule_device_manager.motor_move_by(id + 10, pos, speed, 0);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("mini_servo_move_forward", "(id,torque)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t id     = atoi(paraV[0]);
    int32_t torque = atoi(paraV[1]);
    torque         = abs(torque);
    int32_t ecode  = g_zmodule_device_manager.motor_move_to_with_torque(id + 10, 0, torque);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("mini_servo_move_to", "(id,pos,speed,torque)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t id     = atoi(paraV[0]);
    int32_t pos    = atoi(paraV[1]);
    int32_t speed  = atoi(paraV[2]);
    int32_t torque = atoi(paraV[3]);
    int32_t ecode  = g_zmodule_device_manager.motor_move_to(id + 10, pos, speed, 0);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("mini_servo_position_calibrate", "(id,pos)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t id    = atoi(paraV[0]);
    int32_t pos   = atoi(paraV[1]);
    int32_t ecode = g_zmodule_device_manager.motor_set_current_pos_by_change_shift(id + 10, pos);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("mini_servo_read_status", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t id    = atoi(paraV[0]);
    int32_t pos   = 0;
    int32_t ecode = g_zmodule_device_manager.motor_read_pos(id + 10, &pos);
    ack->setInt32Ack(ecode, pos);
  });

  g_cmdScheduler.regCMD("mini_servo_rotate", "(id,speed,torque,time)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t id     = atoi(paraV[0]);
    int32_t speed  = atoi(paraV[1]);
    int32_t torque = atoi(paraV[2]);
    int32_t time   = atoi(paraV[3]);

    int     direction = speed > 0 ? 1 : -1;
    int32_t ecode     = g_zmodule_device_manager.motor_rotate(id + 10, direction, speed, time);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("mini_servo_stop", "(id,stop_type)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t id        = atoi(paraV[0]);
    int32_t stop_type = atoi(paraV[1]);
    int32_t ecode     = g_zmodule_device_manager.module_stop(id + 10);
    ack->setNoneAck(ecode);
  });

#if 0
  step_motor_45_get_pos (id)
  step_motor_45_move_by (id,pos)
  step_motor_45_move_to (id,pos)
  step_motor_45_rotate (id,direction)
  step_motor_45_stop (id)
  step_motor_45_zero_calibration (id)
#endif

  g_cmdScheduler.regCMD("step_motor_45_get_pos", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t pos   = 0;
    int     id    = atoi(paraV[0]);
    int32_t ecode = g_zmodule_device_manager.motor_read_pos(id + 20, &pos);
    ack->setInt32Ack(ecode, pos);
  });

  g_cmdScheduler.regCMD("step_motor_45_move_by", "(id,pos)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t pos   = atoi(paraV[1]);
    int     id    = atoi(paraV[0]);
    int32_t ecode = g_zmodule_device_manager.motor_move_by(id + 20, pos,0,0);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("step_motor_45_move_to", "(id,pos)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t pos   = atoi(paraV[1]);
    int     id    = atoi(paraV[0]);
    int32_t ecode = g_zmodule_device_manager.motor_move_to(id + 20, pos,0,0);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("step_motor_45_rotate", "(id,direction)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int32_t direction = atoi(paraV[1]);
    int     id        = atoi(paraV[0]);
    int32_t ecode     = g_zmodule_device_manager.motor_rotate(id + 20, direction,0,0);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("step_motor_45_stop", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int     id    = atoi(paraV[0]);
    int32_t ecode = g_zmodule_device_manager.module_stop(id + 20);
    ack->setNoneAck(ecode);
  });

  g_cmdScheduler.regCMD("step_motor_45_zero_calibration", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
    int     id    = atoi(paraV[0]);
    int32_t ecode = g_zmodule_device_manager.motor_move_to_zero_backward(id + 20,0,0,0,0);
    ack->setNoneAck(ecode);
  });


}

extern void step_motor_cmd_reg();
void        Main::run() {
  /*******************************************************************************
   *                                 ϵͳ��ʼ��                                  *
   *******************************************************************************/

  chip_init(&chipcfg);
  zos_cfg_t zoscfg;
  zos_init(&zoscfg);

  /*******************************************************************************
   *                                 ���߳�ʼ��                                  *
   *******************************************************************************/
  auto* cfg = m_zcanCommnaderMaster.createCFG();                                // can��������
  m_zcanCommnaderMaster.init(cfg);                                              // can����
  g_modbusblockhost.initialize(&huart2, &hdma_usart2_tx, &hdma_usart2_rx);      // modbus����
  g_feiteservomotor_bus.initialize(&huart3, &hdma_usart3_rx, &hdma_usart3_tx);  // ���ض�������
  step_motor45_scheduler.initialize(&htim10);
  // �豸��������ʼ��
  g_zmodule_device_manager.initialize(&m_zcanCommnaderMaster);
  // �豸ָ����������ʼ��

  /*******************************************************************************
   *                              �豸�����ͳ�ʼ��                               *
   *******************************************************************************/
  g_main_servo_motor.init(2, &g_modbusblockhost, 1);
  g_xyrobotctrlmodule.initialize(3, &m_zcanCommnaderMaster);
  g_z_step_motor.initialize(4, &m_zcanCommnaderMaster);

  g_mini_servo[0].initialize(11, &g_feiteservomotor_bus, 1);
  g_mini_servo[1].initialize(12, &g_feiteservomotor_bus, 2);
  g_mini_servo[2].initialize(13, &g_feiteservomotor_bus, 3);
  g_mini_servo[3].initialize(14, &g_feiteservomotor_bus, 4);
  g_mini_servo[4].initialize(15, &g_feiteservomotor_bus, 5);
  g_mini_servo[5].initialize(16, &g_feiteservomotor_bus, 6);

  g_step_motor45[0].initialize(21, &step_motor45_scheduler, cfg1);
  g_step_motor45[1].initialize(22, &step_motor45_scheduler, cfg2);
  g_step_motor45[2].initialize(23, &step_motor45_scheduler, cfg3);
  step_motor45_scheduler.start();

  g_zmodule_device_manager.registerModule(&g_main_servo_motor);
  g_zmodule_device_manager.registerModule(&g_xyrobotctrlmodule);
  g_zmodule_device_manager.registerModule(&g_z_step_motor);
  g_zmodule_device_manager.registerModule(&g_mini_servo[0]);
  g_zmodule_device_manager.registerModule(&g_mini_servo[1]);
  g_zmodule_device_manager.registerModule(&g_mini_servo[2]);
  g_zmodule_device_manager.registerModule(&g_mini_servo[3]);
  g_zmodule_device_manager.registerModule(&g_mini_servo[4]);
  g_zmodule_device_manager.registerModule(&g_mini_servo[5]);
  g_zmodule_device_manager.registerModule(&g_step_motor45[0]);
  g_zmodule_device_manager.registerModule(&g_step_motor45[1]);
  g_zmodule_device_manager.registerModule(&g_step_motor45[2]);

  g_intelligent_winding_robot_ctrl.initialize(&g_zmodule_device_manager, &g_cmdScheduler);

  /*******************************************************************************
   *                             �����ַ���ָ������                              *
   *******************************************************************************/
  g_cmdScheduler.initialize(&DEBUG_UART, 1000);  //
  g_zmodule_device_script_cmder_paser.initialize(&g_cmdScheduler, &g_zmodule_device_manager);
  g_taojingchi_screen_service.initialize(
      &huart5, 1,
      /**
       * @brief ˽��Э������
       */
      [this](const char* cmd, int32_t paramN, const char** paraV) {  //
        ZLOGI(TAG, "process cmd:%s", cmd);
      },
      /**
       * @brief �վ�����Ϣ����
       */
      [this](uint8_t* data, size_t len) {});
  regfn();
#if 0
  step_motor_cmd_reg();
#endif
  while (true) {
    OSDefaultSchduler::getInstance()->loop();
    g_cmdScheduler.schedule();
    osDelay(1);
  }
}