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"

#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) { umain(); }
}
/*******************************************************************************
 *                                    ����                                     *
 *******************************************************************************/
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,
};

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;

ModbusBlockHost g_modbusblockhost;
Eq20ServoMotor  g_eq20servomotor;
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;
}

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
  // 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 v   = atoi(argv[3]);
    ZLOGI(TAG, "mini_servo_move_to %d %d %d", id, pos, v);
    g_feiteservomotor.moveTo(id, pos, v, 0);
  });

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

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

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