iflytop_cancmder/usrc/feite_servo_motor.cpp

#include "feite_servo_motor.hpp"

#include <stdint.h>
#include <string.h>

// #include "board.h"
// #include "port.h"
using namespace iflytop;
using namespace std;
using namespace feite;
#define TAG      "FeiTeServoMotor"
#define OVERTIME 30
#define DO(func)                                   \
  if (!(func)) {                                   \
    ZLOGE(TAG, "motor[%d] do %s fail", id, #func); \
    return false;                                  \
  }

static void dumphex(const char* tag, uint8_t* data, uint8_t len) {
  printf("%s:", tag);
  for (int i = 0; i < len; i++) {
    printf("%02x ", data[i]);
  }
  printf("\n");
}
void FeiTeServoMotor::initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx) {
  m_uart    = uart;
  m_hdma_rx = hdma_rx;
  m_hdma_tx = hdma_tx;
}

bool FeiTeServoMotor::ping(uint8_t id) {
  ping_cmd_t  ping_cmd;
  ping_resp_t ping_resp;
  ping_cmd.header   = 0xffff;
  ping_cmd.id       = id;
  ping_cmd.len      = 2;
  ping_cmd.cmd      = kping;
  ping_cmd.checksum = checksum_packet((uint8_t*)&ping_cmd, sizeof(ping_cmd_t));

  return tx_and_rx((uint8_t*)&ping_cmd, sizeof(ping_cmd_t), (uint8_t*)&ping_resp, sizeof(ping_resp_t), OVERTIME);
}

static int16_t getcalibrate(int16_t nowpos, int16_t aftercalibratepos) {
  int16_t calibrate = nowpos - aftercalibratepos;
  while (true) {
    if (calibrate > 2047) {
      calibrate -= 4094;
    } else if (calibrate < -2047) {
      calibrate += 4094;
    } else {
      break;
    }
  }
  return calibrate;
}

bool FeiTeServoMotor::setmode(uint8_t id, run_mode_e runmode) { return write_u8(id, kRegServoRunMode, (uint8_t)runmode); }
bool FeiTeServoMotor::getServoCalibration(uint8_t id, int16_t& poscalibration) { return read_s16(id, kRegServoCalibration, 11, poscalibration); }

run_mode_e FeiTeServoMotor::getmode(uint8_t id) {
  uint8_t data = 0;
  bool    suc  = read_u8(id, kRegServoRunMode, data);
  if (suc) {
    return (run_mode_e)data;
  } else {
    return kMotorMode;
  }
}

bool FeiTeServoMotor::getmode(uint8_t id, run_mode_e& runmode) {
  uint8_t data = 0;
  bool    suc  = read_u8(id, kRegServoRunMode, data);
  runmode      = (run_mode_e)data;
  return suc;
}

bool FeiTeServoMotor::setTorqueSwitch(uint8_t id, bool on) { return write_u8(id, kRegServoTorqueSwitch, on ? 1 : 0); }
bool FeiTeServoMotor::getTorqueSwitch(uint8_t id, bool& on) {
  uint8_t data = 0;
  bool    suc  = read_u8(id, kRegServoTorqueSwitch, data);
  on           = data;
  return suc;
}

bool FeiTeServoMotor::getNowPos(uint8_t id, int16_t& pos) { return read_s16(id, kRegServoCurrentPos, 15, pos); }
bool FeiTeServoMotor::setTargetPos(uint8_t id, int16_t pos) { return write_s16(id, kRegServoTargetPos, 15, pos); }

bool FeiTeServoMotor::triggerAysncWrite(uint8_t id) {
  cmd_header_t* cmd_header = (cmd_header_t*)m_txbuf;
  cmd_header->header       = 0xffff;
  cmd_header->id           = id;
  cmd_header->len          = 2;
  cmd_header->cmd          = 5;
  cmd_header->data[0]      = checksum((uint8_t*)cmd_header, sizeof(cmd_header_t) + 1);

  // HAL_UART_Transmit(m_uart, m_txbuf, sizeof(cmd_header_t) + 1, 1000);
  return true;
}

bool FeiTeServoMotor::rotate(uint8_t id, int16_t speed, uint16_t torque) {
  DO(setmode(id, kMotorMode));
  if (torque == 0) torque = 1000;
  DO(write_u16(id, kRegServoTorqueLimit, torque));
  DO(write_s16(id, kRegServoRunSpeed, 15, speed));
  return true;
}

bool FeiTeServoMotor::moveTo(uint8_t id, int16_t pos, int16_t speed, uint16_t torque) {
  /**
   * @brief ����Ť��
   */
  DO(setmode(id, kServoMode));
  if (torque == 0) torque = 1000;
  DO(write_u16(id, kRegServoTorqueLimit, torque));
  DO(write_s16(id, kRegServoRunSpeed, 15, speed));
  DO(setTargetPos(id, pos));
  return true;
}
uint16_t abs16(int16_t val) {
  if (val < 0) {
    return -val;
  } else {
    return val;
  }
}
bool FeiTeServoMotor::moveWithTorque(uint8_t id, int16_t torque) {
  DO(setmode(id, kOpenMotorMode));
  if (torque == 0) torque = 1000;
  DO(write_u16(id, kRegServoTorqueLimit, abs16(torque)));
  DO(write_s16(id, kRegServoRunTime, 15, torque));
  return true;
}

static int16_t tosign16(uint16_t* d, int signoff) {
  uint16_t sign = (*d >> signoff) & 0x01;
  uint16_t val  = *d & (~(1 << signoff));
  if (sign == 0) {
    return val;
  } else {
    return -val;
  }
}

bool FeiTeServoMotor::read_status(uint8_t id, status_t* status) {
  // kRegServoCurrentPos
  bool suc    = read_reg(id, kRegServoCurrentPos, (uint8_t*)status, sizeof(status_t));
  status->vel = tosign16((uint16_t*)&status->vel, 15);
  if (!suc) return false;
  return true;
}
bool FeiTeServoMotor::read_detailed_status(uint8_t id, detailed_status_t* detailed_status) {
  bool suc = read_reg(id, kRegServoCurrentPos, (uint8_t*)detailed_status, sizeof(*detailed_status));
  if (!suc) return false;
  detailed_status->vel    = tosign16((uint16_t*)&detailed_status->vel, 15);
  detailed_status->torque = tosign16((uint16_t*)&detailed_status->torque, 10);

  return true;
}
void FeiTeServoMotor::dump_status(status_t* status) {
  ZLOGI(TAG, "===========status===========");
  ZLOGI(TAG, "= status->pos        :%d", status->pos);
  ZLOGI(TAG, "= status->vel        :%d", status->vel);
  ZLOGI(TAG, "= status->torque     :%d", status->torque);
  ZLOGI(TAG, "=");
}
#define BIT_IN_BYTE(byte, off) ((byte >> off) & 0x01)
void FeiTeServoMotor::dump_detailed_status(detailed_status_t* detailed_status) {
  ZLOGI(TAG, "===========detailed_status===========");
  ZLOGI(TAG, "= detailed_status->pos        :%d", detailed_status->pos);
  ZLOGI(TAG, "= detailed_status->vel        :%d", detailed_status->vel);
  ZLOGI(TAG, "= detailed_status->torque     :%d", detailed_status->torque);
  ZLOGI(TAG, "= detailed_status->voltage    :%d", detailed_status->voltage);
  ZLOGI(TAG, "= detailed_status->temperature:%d", detailed_status->temperature);
  ZLOGI(TAG, "= detailed_status->state      :%d:%d:%d:%d:%d:%d", BIT_IN_BYTE(detailed_status->state, 0), BIT_IN_BYTE(detailed_status->state, 1), BIT_IN_BYTE(detailed_status->state, 2),
        BIT_IN_BYTE(detailed_status->state, 3), BIT_IN_BYTE(detailed_status->state, 4), BIT_IN_BYTE(detailed_status->state, 5));
  ZLOGI(TAG, "= detailed_status->moveflag   :%d", detailed_status->moveflag);
  ZLOGI(TAG, "= detailed_status->current    :%d", detailed_status->current);
  ZLOGI(TAG, "=");
}
bool FeiTeServoMotor::getMoveFlag(uint8_t id, uint8_t& moveflag) { return read_u8(id, kRegServoMoveFlag, moveflag); }

bool FeiTeServoMotor::reCalibration(int id, int16_t pos) {
  if (pos < 0 || pos > 4095) {
    ZLOGE(TAG, "reCalibration pos:%d out of range", pos);
    return false;
  }
  /**
   * @brief �ر�Ť�ؿ��أ���ֹ�����˶�
   */
  setTorqueSwitch(id, false);
  /**
   * @brief ���õ�ǰģʽΪλ��ģʽ
   */
  DO(setmode(id, kServoMode));

  int16_t curpos;
  DO(getNowPos(id, curpos));
  int16_t curcalibrate;
  DO(getServoCalibration(id, curcalibrate));
  int16_t realpos      = curpos + curcalibrate;
  int16_t newcalibrate = getcalibrate(realpos, pos);

  ZLOGI(TAG, "reCalibration id:%d curpos:%d curcalibrate:%d realpos:%d newcalibrate:%d", id, curpos, curcalibrate, realpos, newcalibrate);
  /**
   * @brief д���µ�У׼ֵ
   */
  DO(write_u8(id, kRegServoLockFlag, 0));
  DO(write_s16(id, kRegServoCalibration, 11, newcalibrate));
  DO(write_u8(id, kRegServoLockFlag, 1));
  /**
   * @brief ��������Ŀ��λ��Ϊ��ǰ����λ��
   */
  int16_t nowpos;
  DO(getNowPos(id, nowpos));
  ZLOGI(TAG, "reCalibration id:%d nowpos:%d:%d", id, nowpos, pos);
  DO(setTargetPos(id, pos));
  return true;
}

/*******************************************************************************
 *                                  BASEFUNC                                   *
 *******************************************************************************/
bool FeiTeServoMotor::write_u8(uint8_t id, feite::reg_add_e add, uint8_t regval) { return write_reg(id, false, add, &regval, 1); }
bool FeiTeServoMotor::read_u8(uint8_t id, feite::reg_add_e add, uint8_t& regval) { return read_reg(id, add, &regval, 1); }
bool FeiTeServoMotor::write_u16(uint8_t id, feite::reg_add_e add, uint16_t regval) { return write_reg(id, false, add, (uint8_t*)&regval, 2); }
bool FeiTeServoMotor::read_u16(uint8_t id, feite::reg_add_e add, uint16_t& regval) { return read_reg(id, add, (uint8_t*)&regval, 2); }
bool FeiTeServoMotor::async_write_u8(uint8_t id, feite::reg_add_e add, uint8_t regval) { return write_reg(id, true, add, &regval, 1); }
bool FeiTeServoMotor::async_write_u16(uint8_t id, feite::reg_add_e add, uint16_t regval) { return write_reg(id, true, add, (uint8_t*)&regval, 2); }
bool FeiTeServoMotor::async_write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t regval) {
  uint16_t val = 0;
  if (regval >= 0) {
    val = regval;
  } else {
    val = -regval;
    val |= (1 << signbitoff);
  }
  return async_write_u16(id, add, val);
}

bool FeiTeServoMotor::read_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t& regval) {
  uint16_t val = 0;
  bool     ret = read_u16(id, add, val);
  if (!ret) return false;
  uint8_t  sign    = (val >> signbitoff) & 0x01;
  uint16_t realval = val & (~(1 << signbitoff));
  if (sign == 0) {
    regval = realval;
  } else {
    regval = -realval;
  }
  return true;
}
bool FeiTeServoMotor::write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t regval) {
  uint16_t val = 0;
  if (regval >= 0) {
    val = regval;
  } else {
    val = -regval;
    val |= (1 << signbitoff);
  }
  return write_u16(id, add, val);
}
bool FeiTeServoMotor::write_reg(uint8_t id, bool async, uint8_t add, uint8_t* data, uint8_t len) {  //
  ZLOGI(TAG, "write_reg id:%d add:%d len:%d", id, add, len);
  cmd_header_t*     cmd_header     = (cmd_header_t*)m_txbuf;
  receipt_header_t* receipt_header = (receipt_header_t*)m_rxbuf;
  cmd_header->header               = 0xffff;
  cmd_header->id                   = id;
  cmd_header->len                  = 3 + len;  // 3 == cmd + add + checksum
  cmd_header->cmd                  = async ? kasyncWrite : kwrite;
  cmd_header->data[0]              = add;
  memcpy(&cmd_header->data[1], data, len);

  int txpacketlen = sizeof(cmd_header_t) + 1 + len + 1;
  int rxpacketlen = sizeof(receipt_header_t) + 1;

  uint8_t checksum         = checksum_packet((uint8_t*)cmd_header, txpacketlen);
  m_txbuf[txpacketlen - 1] = checksum;
  if (!tx_and_rx(m_txbuf, txpacketlen, m_rxbuf, rxpacketlen, OVERTIME)) {
    ZLOGE(TAG, "write_reg fail,overtime");
    return false;
  }
  if (!(receipt_header->header == 0xffff && receipt_header->id == id)) {
    ZLOGE(TAG, "write_reg fail,receipt header error");
    return false;
  }
  return true;
}
bool FeiTeServoMotor::read_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t len) {
  //  return false;
  cmd_header_t*     cmd_header     = (cmd_header_t*)m_txbuf;
  receipt_header_t* receipt_header = (receipt_header_t*)m_rxbuf;
  cmd_header->header               = 0xffff;
  cmd_header->id                   = id;
  cmd_header->len                  = 3 + 1;  // 4 == cmd + add + checksum + readlen
  cmd_header->cmd                  = kread;
  cmd_header->data[0]              = add;
  cmd_header->data[1]              = len;

  int txpacketlen = sizeof(cmd_header_t) + 3;
  int rxpacketlen = sizeof(receipt_header_t) + 1 + len;

  uint8_t checksum = checksum_packet((uint8_t*)cmd_header, txpacketlen);

  m_txbuf[txpacketlen - 1] = checksum;
  if (!tx_and_rx(m_txbuf, txpacketlen, m_rxbuf, rxpacketlen, OVERTIME)) {
    return false;
  }
  if (!(receipt_header->header == 0xffff && receipt_header->id == id)) {
    ZLOGE(TAG, "read_reg fail,receipt header error");
    return false;
  }
  memcpy(data, receipt_header->data, len);
  return true;
}
bool FeiTeServoMotor::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, uint16_t overtimems) {
  uint32_t enter_ticket = HAL_GetTick();
  dumphex("tx:", tx, txdatalen);

  HAL_UART_Transmit(m_uart, tx, txdatalen, 1000);
  HAL_UART_Receive_DMA(m_uart, (uint8_t*)rx, expectrxsize);


  bool overtime_flag = false;
  while (HAL_UART_GetState(m_uart) == HAL_UART_STATE_BUSY_RX ||  //
         HAL_UART_GetState(m_uart) == HAL_UART_STATE_BUSY_TX_RX) {
    osDelay(1);
    int rxsize = expectrxsize - __HAL_DMA_GET_COUNTER(m_hdma_rx);
    if (rxsize == expectrxsize) {
      dumphex("rx:", rx, expectrxsize);
      break;
    }
    if (zos_haspassedms(enter_ticket) > overtimems) {
      if (expectrxsize != 0 && rxsize != 0) {
        ZLOGW(TAG, "txandrx overtime rxsize:%d != expect_size:%d", rxsize, expectrxsize);
      }
      overtime_flag = true;
      break;
    }
  }
  HAL_UART_DMAStop(m_uart);
  if (overtime_flag) {
    return false;
  }
  return true;
}

bool FeiTeServoMotor::readversion(uint8_t id, uint8_t& mainversion, uint8_t& subversion, uint8_t& miniserv_mainversion, uint8_t& miniserv_subversion) {
  uint8_t data = 0;
  DO(read_u8(id, kRegFirmwareMainVersion, data));
  mainversion = data;
  DO(read_u8(id, kRegFirmwareSubVersion, data));
  subversion = data;
  DO(read_u8(id, kRegServoMainVersion, data));
  miniserv_mainversion = data;
  DO(read_u8(id, kRegServoSubVersion, data));
  miniserv_subversion = data;
  return true;
}

uint8_t FeiTeServoMotor::checksum_packet(uint8_t* data, uint8_t len) { return checksum(&data[2], len - 3); }
uint8_t FeiTeServoMotor::checksum(uint8_t* data, uint8_t len) {
  // CheckSum=~(ID+Length+Instruction+Parameter1+...ParameterN
  uint16_t sum = 0;
  for (int i = 0; i < len; i++) {
    sum += data[i];
  }
  return ~(sum & 0xff);
}