medical_beauty_photography/main/motor_drive.c

#include "motor_drive.h"

#include "ble_parse_data.h"
#include "driver/uart.h"
#include "esp_log.h"

#define MOTOR_DRIVE "MOTOR_DRIVE"

#define uart_num UART_NUM_1
#define tx_io_num 1
#define rx_io_num 2
#define buffer_size 128
#define uart_read_time_ms 200
#define MOTOR_ID 0X01

void motor_drive_uart_init() {
  uart_config_t uart_config = {
      .baud_rate = 115200,
      .data_bits = UART_DATA_8_BITS,
      .parity = UART_PARITY_DISABLE,
      .stop_bits = UART_STOP_BITS_1,
      .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
      .source_clk = UART_SCLK_APB,
  };
  ESP_ERROR_CHECK(uart_driver_install(uart_num, buffer_size * 2, 0, 0, NULL, 0));

  ESP_ERROR_CHECK(uart_param_config(uart_num, &uart_config));

  ESP_ERROR_CHECK(uart_set_pin(uart_num, tx_io_num, rx_io_num, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
}

void motor_drive_turn(int direction, int speed_level, double position) {
  if ((direction > 2) || (direction < 0)) {
    ESP_LOGW(MOTOR_DRIVE, "Direction out of range");
  }
  if ((speed_level > 9) || (speed_level < 0)) {
    ESP_LOGW(MOTOR_DRIVE, "Speed level out of range");
  }
  if ((position > 360) || (position <= 0)) {
    ESP_LOGW(MOTOR_DRIVE, "Position out of range");
  }
  motor_drive_set_packages_ctr(position, direction);
  uart_write_bytes(uart_num, "test", strlen("test"));
}

double motor_drive_read_encoder() {
  size_t encoder_buffer_size = 5;
  uint8_t buffer[5] = {0X3E, 0X90, MOTOR_ID, 0X00, 0XCF};
  uint16_t encoder_data = 0;
  char *notify_err = "read encoder error";

  // Generate cmd
  // encoder_buffer_size = motor_drive_buffer_cmd_generate(buffer, 0x90, 4, 0X5A97FF00);
  // if (encoder_buffer_size == 0) {
  //   ESP_LOGW(MOTOR_DRIVE, "generate_buffer_size null");
  //   return -1;
  // }

  // Send cmd
  uart_flush(uart_num);
  uart_write_bytes(uart_num, (const char *)buffer, encoder_buffer_size);

  encoder_buffer_size = 0;
  memset(buffer, 0, sizeof(uint8_t) * 5);

  // Wait receive
  encoder_buffer_size = uart_read_bytes(uart_num, buffer, 12, uart_read_time_ms / portTICK_RATE_MS);
  if (encoder_buffer_size != 12 || buffer[0] != 0X3E) {
    ESP_LOGW(MOTOR_DRIVE, "encoder size:%d,buffer[0] = 0X%x", encoder_buffer_size, buffer[0]);
    bluetooth_active_notify((uint8_t *)notify_err, strlen(notify_err));
    return -1.0;
  }

  // Parse receive
  // motor_drive_buffer_cmd_parse(buffer);

  encoder_data = buffer[5] + (buffer[6] << 8);

  // parse motor usart
  return ((double)encoder_data / 100.0);
}

void motor_drive_set_packages_data_max64bit(uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data) {
  uint8_t buffer[20] = {0};

  motor_drive_buffer_cmd_generate(buffer, cmd, buffer_data_size, buffer_data);
}

uint8_t motor_drive_set_packages_ctr(double position, int direction) {
  int position_int = 0;
  uint8_t position_remainder = 0;
  uint8_t position_buffer_size = 5;  //从第五位开始(低位)
  uint8_t checksum = 0;
  uint8_t buffer[10] = {0x3E, 0XA7, MOTOR_ID, 0X04, 0XEA, 0X00, 0X00, 0X00, 0X00, 0X00};
  char *notify_err = "set size error";

  position_int = position * 100;
  if (direction == 2) {
    position_int = 0 - position_int;
  }

  if (position_int != 0) {
    if (position_int > 0) {  // Positive number
      while ((position_int / 0X100) > 0) {
        position_remainder = position_int & 0XFF;
        buffer[position_buffer_size] = position_remainder;
        position_buffer_size += 1;
        position_int = position_int >> 8;
        checksum += position_remainder;
      }
      buffer[position_buffer_size] = position_int;
      checksum += position_int;
      checksum = checksum & 0XFF;
      buffer[9] = checksum;
    } else {  // Negative
      while ((position_int / 0X100) < 0) {
        position_remainder = position_int & 0XFF;
        buffer[position_buffer_size] = position_remainder;
        position_buffer_size += 1;
        position_int = position_int >> 8;
        checksum += position_remainder;
      }
      buffer[position_buffer_size] = position_int;
      position_buffer_size += 1;
      while (position_buffer_size != 9) {
        buffer[position_buffer_size] = 0XFF;
        position_buffer_size += 1;
        checksum += 0XFF;
      }
      checksum += position_int;
      checksum = checksum & 0XFF;
      buffer[9] = checksum;
    }
  }

  // Send cmd
  uart_flush(uart_num);
  uart_write_bytes(uart_num, buffer, 10);

  position_buffer_size = 0;
  memset(buffer, 0, sizeof(uint8_t) * 10);

  // Wait uart receive,if time out return error and output log
  position_buffer_size = uart_read_bytes(uart_num, buffer, 13, uart_read_time_ms / portTICK_RATE_MS);
  if (position_buffer_size != 13 || buffer[0] != 0X3E) {
    ESP_LOGW(MOTOR_DRIVE, "set motor size error ,buffer_size:%d,buffer[0] = 0X%x", position_buffer_size, buffer[0]);
    bluetooth_active_notify((uint8_t *)notify_err, strlen(notify_err));
    return 1;
  }

  // Parse receive
  // motor_drive_buffer_cmd_parse(buffer);
  return 0;
}

// Notice the strbuffer size
void motor_drive_hex_to_str(const char *hex, int hex_len, char *str) {
  int i, pos = 0;
  ESP_LOGI(MOTOR_DRIVE, "0X%X", hex[0]);
  for (i = 0; i < hex_len; i++)

  {
    sprintf(str + pos, "%02x", hex[i]);

    pos += 2;
  }
  ESP_LOGI(MOTOR_DRIVE, "0X%X", hex[0]);
}

size_t motor_drive_buffer_cmd_generate(uint8_t *buffer, uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data) {
  uint8_t i = 0;
  uint8_t checksum = 0;
  uint8_t buffer_data_uint8 = 0;
  uint8_t strbuffer[20] = {'\0'};
  uint8_t hex_to_str_size = buffer_data_size;
  if (buffer == NULL) {
    ESP_LOGW(MOTOR_DRIVE, "buffer nil ,init error");
    return 0;
  }
  buffer[0] = 0X3E;
  buffer[1] = cmd;
  buffer[2] = 0X1;
  buffer[3] = buffer_data_size;
  buffer[4] = (0X3E + cmd + 0X1 + buffer_data_size) % 0XFF;

  if (buffer_data_size > 0) {
    hex_to_str_size = buffer_data_size + 1;
    for (i = 0; i < buffer_data_size; i++) {
      buffer_data_uint8 = buffer_data;
      buffer_data = buffer_data >> 8;
      buffer[5 + i] = buffer_data_uint8;
      checksum += buffer_data_uint8;
    }
    buffer[5 + buffer_data_size] = (checksum % 0XFF);
  }

  motor_drive_hex_to_str((char *)buffer, (5 + hex_to_str_size), (char *)strbuffer);
  ESP_LOGI(MOTOR_DRIVE, "%s", strbuffer);
  // motor_drive_buffer_cmd_parse(buffer);

  return (5 + hex_to_str_size);
}

void motor_drive_buffer_cmd_parse(uint8_t *buffer) {
  // uint8_t i = 0;
  // uint16_t temp_data_arr[(buffer[3] / 2)];
  // ESP_LOGI(MOTOR_DRIVE, "=====buffer data size %d=====", buffer[3]);
  if (buffer == NULL) {
    ESP_LOGW(MOTOR_DRIVE, "cmd parse buffer null");
    return;
  }

  if (buffer[0] != 0X3E || buffer[2] != MOTOR_ID) {
    ESP_LOGW(MOTOR_DRIVE, "cmd parse buffer error");
    return;
  }

  // for (i = 0; i < (buffer[3]); i += 2) {
  // temp_data_arr[i] = buffer[5 + i] + (buffer[5 + i + 1] << 8);
  // ESP_LOGI(MOTOR_DRIVE, "%d\n", temp_data_arr[i]);
  // }
}

u_int8_t motor_drive_set_motor_current_size() {
  size_t set_current_buffer_size = 5;
  uint8_t buffer[5] = {0X3E, 0X19, MOTOR_ID, 0X00, 0X58};
  char *notify_err = "Set current size err";
  uart_flush(uart_num);
  uart_write_bytes(uart_num, buffer, 5);

  set_current_buffer_size = 0;
  memset(buffer, 0, sizeof(uint8_t) * 5);

  // Wait receive
  set_current_buffer_size = uart_read_bytes(uart_num, buffer, 5, uart_read_time_ms / portTICK_RATE_MS);
  if (set_current_buffer_size != 5 || buffer[0] != 0X3E) {
    ESP_LOGW(MOTOR_DRIVE, "Set current size:%d,buffer[0] = 0X%x", set_current_buffer_size, buffer[0]);
    bluetooth_active_notify((uint8_t *)notify_err, strlen(notify_err));
    return 1;
  }

  return 0;
}

double motor_drive_read_single_turn_angle() {
  uint16_t turn_angle_data = 0;
  size_t turn_angle_buffer_size = 5;
  uint8_t buffer[5] = {0X3E, 0X94, MOTOR_ID, 0X00, 0XD3};
  char *notify_err = "Turn angle size err";

  uart_flush(uart_num);
  uart_write_bytes(uart_num, buffer, 5);

  turn_angle_buffer_size = 0;
  memset(buffer, 0, sizeof(uint8_t) * 5);

  // Wait receive
  turn_angle_buffer_size = uart_read_bytes(uart_num, buffer, 8, uart_read_time_ms / portTICK_RATE_MS);
  if (turn_angle_buffer_size != 8 || buffer[0] != 0X3E) {
    ESP_LOGW(MOTOR_DRIVE, "Turn angle size:%d,buffer[0] = 0X%x", turn_angle_buffer_size, buffer[0]);
    bluetooth_active_notify((uint8_t *)notify_err, strlen(notify_err));
    return -1;
  }

  // Parse receive
  // motor_drive_buffer_cmd_parse(buffer);

  turn_angle_data = buffer[5] + (buffer[6] << 8);

  return ((double)turn_angle_data / 100.0);
}

void motor_drive_set_motor_to_angle(int rotation_direction, double position, int speed_level) {
  int position_int = 0;
  uint8_t position_remainder = 0;
  uint8_t position_buffer_size = 6;  //从第五位开始(低位)
  uint8_t checksum = 0;
  uint32_t limit_speed = 0;
  uint8_t buffer[14] = {0x3E, 0XA6, MOTOR_ID, 0X08, 0XED, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00};
  char *notify_err = "set size error";

  // forward
  if ((rotation_direction == 1)) {
    buffer[5] = 0;
    checksum += 0;
  }

  // reverse
  if ((rotation_direction == 2)) {
    buffer[5] = 1;
    checksum += 1;
  }

  position_int = position * 100;
  position_int = position_int % 36000;

  if (position_int != 0) {
    while ((position_int / 0X100) > 0) {
      position_remainder = position_int & 0XFF;
      buffer[position_buffer_size] = position_remainder;
      position_buffer_size += 1;
      position_int = position_int >> 8;
      checksum += position_remainder;
    }
    buffer[position_buffer_size] = position_int;
    checksum += position_int;
  }

  if (speed_level <= 9) {
    limit_speed = speed_level * 360;
    position_buffer_size = 9;
  }

  if (limit_speed != 0) {
    while ((limit_speed / 0X100) > 0) {
      position_remainder = limit_speed & 0XFF;
      buffer[position_buffer_size] = position_remainder;
      position_buffer_size += 1;
      limit_speed = limit_speed >> 8;
      checksum += position_remainder;
    }
    buffer[position_buffer_size] = limit_speed;
    checksum += limit_speed;
  }

  checksum = checksum & 0XFF;
  buffer[13] = checksum;

  // Send cmd
  uart_flush(uart_num);
  uart_write_bytes(uart_num, buffer, 14);

  position_buffer_size = 0;
  memset(buffer, 0, sizeof(uint8_t) * 14);

  // Wait uart receive, if time out return error and output log
  position_buffer_size = uart_read_bytes(uart_num, buffer, 13, uart_read_time_ms / portTICK_RATE_MS);
  if (position_buffer_size != 13 || buffer[0] != 0X3E) {
    ESP_LOGW(MOTOR_DRIVE, "set motor size error ,buffer_size:%d,buffer[0] = 0X%x", position_buffer_size, buffer[0]);
    bluetooth_active_notify((uint8_t *)notify_err, strlen(notify_err));
  }

  // Parse receive
  motor_drive_buffer_cmd_parse(buffer);
  return;
}