graphite_digester_zancmder_debug_board/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"

#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    = false,
    .enable_max_pos_limit = false,
    .mirror               = true,

    .zeroPin       = PinNull,
    .zeroPinMirror = false,

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

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

    .zeroPin       = PinNull,
    .zeroPinMirror = false,

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

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

    .zeroPin       = PinNull,
    .zeroPinMirror = false,

    .driverPin       = {PG6, PG7, PG8, PC6},
    .driverPinMirror = true,
};

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,
};

/*******************************************************************************
 *                                    ����                                     *
 *******************************************************************************/
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;
}  // namespace iflytop

void regfn() {
  // setzero
  // set4095
  // moveto

  cmdScheduler.registerCmd("mini_servo_set_zero", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(1);
    int id = atoi(argv[1]);
    ZLOGI(TAG, "mini_servo_set_zero %d", id);
    g_feiteservomotor.reCalibration(id, 0);
    ZLOGI(TAG, "mini_servo_set_zero %d done", id);
  });
  cmdScheduler.registerCmd("mini_servo_set_4095", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(1);
    int id = atoi(argv[1]);
    ZLOGI(TAG, "mini_servo_set_4095 %d", id);
    g_feiteservomotor.reCalibration(id, 4095);
    ZLOGI(TAG, "mini_servo_set_4095 %d done", id);
  });
  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 torque = atoi(argv[3]);

    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);
    }
  });

  cmdScheduler.registerCmd("sleep_ms", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(1);
    osDelay(atoi(argv[1]));
  });
}

extern "C" {
extern DMA_HandleTypeDef hdma_usart3_rx;
extern DMA_HandleTypeDef hdma_usart3_tx;
}

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

  chip_init(&chipcfg);

  zos_cfg_t zoscfg;
  zos_init(&zoscfg);

  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[6]);
  //   g_step_motor45_1.rotate(true, 1000);

  step_motor45_scheduler.start();

  cmdScheduler.initialize(&DEBUG_UART, 1000);

  g_modbusblockhost.initialize(&huart2);
  g_eq20servomotor.init(&g_modbusblockhost);

  g_feiteservomotor.initialize(&huart3, &hdma_usart3_rx, &hdma_usart3_tx);

  step_motor_cmd_reg();
  regfn();
  while (true) {
    OSDefaultSchduler::getInstance()->loop();
    cmdScheduler.schedule();
    m_IflytopCanMaster.periodicJob();
    // m_IflytopCanMaster.writeReg(1,1,1,10);
    // osDelay(1);
  }
}