From aca6089deccc4c6d22ad6053db928a29e0e88f48 Mon Sep 17 00:00:00 2001 From: zwsd Date: Sun, 24 Jul 2022 12:39:11 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8E=A7=E5=88=B6=E6=8C=87=E4=BB=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- main/main.c | 20 +++++++++------- main/motor_drive.c | 70 +++++++++++++++++++++++++++++++++++++++++++++++++++--- main/motor_drive.h | 63 +++++++++++++++++++++++++----------------------- 3 files changed, 111 insertions(+), 42 deletions(-) diff --git a/main/main.c b/main/main.c index 70b38c8..17816fd 100644 --- a/main/main.c +++ b/main/main.c @@ -57,16 +57,18 @@ void app_main(void) { // char* test_str = "This is a test string.\n"; while (true) { - bluetooth_gatts_try_process_data(); - if (s_bluetooth_processer.auto_report_flag) { - receipt_json_get_status(); - s_bluetooth_processer.auto_report_flag = false; - } + // bluetooth_gatts_try_process_data(); + // if (s_bluetooth_processer.auto_report_flag) { + // receipt_json_get_status(); + // s_bluetooth_processer.auto_report_flag = false; + // } - if ((motor_drive_read_encoder() == s_bluetooth_processer.position) && s_bluetooth_processer.motor_drive_turn_flag == true) { - ESP_LOGI("test", "info log ok\n"); - s_bluetooth_processer.motor_drive_turn_flag = false; - } + // if ((motor_drive_read_encoder() == s_bluetooth_processer.position) && s_bluetooth_processer.motor_drive_turn_flag == true) { + // ESP_LOGI("test", "info log ok\n"); + // s_bluetooth_processer.motor_drive_turn_flag = false; + // } + + motor_drive_set_packages_ctr(35.68); // uart_write_bytes(UART_NUM_2, (const char*)test_str, strlen(test_str)); // ets_delay_us(1000000); diff --git a/main/motor_drive.c b/main/motor_drive.c index cc8b2a2..f0e28d5 100644 --- a/main/motor_drive.c +++ b/main/motor_drive.c @@ -1,10 +1,30 @@ #include "motor_drive.h" +#include "driver/uart.h" #include "esp_log.h" #define MOTOR_DRIVE "MOTOR_DRIVE" -void motor_drive_uart_init() {} +#define uart_num UART_NUM_2 +#define tx_io_num 4 +#define rx_io_num 5 +#define buffer_size 128 + +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)) { @@ -16,8 +36,52 @@ void motor_drive_turn(int direction, int speed_level, double position) { if ((position > 360) || (position <= 0)) { ESP_LOGW(MOTOR_DRIVE, "Position out of range"); } - - + motor_drive_set_packages_ctr(position); + uart_write_bytes(uart_num, "test", strlen("test")); } double motor_drive_read_encoder() { return 0.0; } + +void motor_drive_set_packages() {} + +void motor_drive_set_packages_ctr(double position) { + 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, 0X01, 0X04, 0XEA, 0X00, 0X00, 0X00, 0X00, 0X00}; + uint8_t strbuffer[20] = {0}; + + position_int = position * 100; + + if (position_int != 0) { + while ((position_int / 256) > 0) { + position_remainder = position_int % 256; + buffer[position_buffer_size] = position_remainder; + position_buffer_size += 1; + position_int = position_int / 256; + checksum += position_remainder; + } + buffer[position_buffer_size] = position_int; + checksum += position_int; + checksum %= 256; + buffer[9] = checksum; + } + for (int i = 0; i < 10; i++) { + ESP_LOGI(MOTOR_DRIVE, "%d", buffer[i]); + } + motor_drive_hex_to_str((char *)buffer, 10, (char *)strbuffer); + ESP_LOGI(MOTOR_DRIVE, "%s", strbuffer); +} + +void motor_drive_hex_to_str(char *hex, int hex_len, char *str) { + int i, pos = 0; + + for (i = 0; i < hex_len; i++) + + { + sprintf(str + pos, "%02x", hex[i]); + + pos += 2; + } +} \ No newline at end of file diff --git a/main/motor_drive.h b/main/motor_drive.h index f0f06c0..c674aa2 100644 --- a/main/motor_drive.h +++ b/main/motor_drive.h @@ -5,37 +5,40 @@ #include typedef enum { - RMDRs485CMDOrder30 = 0x30, //读取PID参数命令 - RMDRs485CMDOrder31 = 0x31, //写入PID参数到RAM命令 - RMDRs485CMDOrder32 = 0x32, //写入PID参数到ROM命令 - RMDRs485CMDOrder33 = 0x33, //读取加速度命令 - RMDRs485CMDOrder34 = 0x34, //写入加速度到RAM命令 - RMDRs485CMDOrder90 = 0x90, //读取编码器命令 - RMDRs485CMDOrder91 = 0x91, //写入编码器值到ROM作为电机零点命令 - RMDRs485CMDOrder19 = 0x19, //写入当前位置到ROM作为电机零点命令 - RMDRs485CMDOrder92 = 0x92, //读取多圈角度命令 - RMDRs485CMDOrder94 = 0x94, //读取单圈角度命令 - RMDRs485CMDOrder95 = 0x95, //清除电机角度命令(设置电机初始位置) - RMDRs485CMDOrder9A = 0x9A, //读取电机状态1和错误标志命令 - RMDRs485CMDOrder9B = 0x9B, //清除电机错误标志命令 - RMDRs485CMDOrder9C = 0x9C, //读取电机状态2命令 - RMDRs485CMDOrder9D = 0x9D, //读取电机状态3命令 - RMDRs485CMDOrder80 = 0x80, //电机关闭命令 - RMDRs485CMDOrder81 = 0x81, //电机停止命令 - RMDRs485CMDOrder88 = 0x88, //电机运行命令 - RMDRs485CMDOrderA1 = 0xA1, //转矩闭环控制命令 - RMDRs485CMDOrderA2 = 0xA2, //速度闭环控制命令 - RMDRs485CMDOrderA3 = 0xA3, //多圈位置闭环控制命令1 - RMDRs485CMDOrderA4 = 0xA4, //多圈位置闭环控制命令2 - RMDRs485CMDOrderA5 = 0xA5, //单圈位置闭环控制命令1 - RMDRs485CMDOrderA6 = 0xA6, //单圈位置闭环控制命令2 - RMDRs485CMDOrderA7 = 0xA7, //增量位置闭环控制命令1 - RMDRs485CMDOrderA8 = 0xA8, //增量位置闭环控制命令2 - RMDRs485CMDOrder12 = 0x12, //读取驱动和电机型号命令 - RMDRs485CMDOrderC2 = 0xC2, //读取多圈角度命令2 - RMDRs485CMDOrderD8 = 0xD8, //增量位置闭环控制命令2 + RMDRs485CMDOrder30 = 0x30, //读取PID参数命令 + RMDRs485CMDOrder31 = 0x31, //写入PID参数到RAM命令 + RMDRs485CMDOrder32 = 0x32, //写入PID参数到ROM命令 + RMDRs485CMDOrder33 = 0x33, //读取加速度命令 + RMDRs485CMDOrder34 = 0x34, //写入加速度到RAM命令 + RMDRs485CMDOrder90 = 0x90, //读取编码器命令 + RMDRs485CMDOrder91 = 0x91, //写入编码器值到ROM作为电机零点命令 + RMDRs485CMDOrder19 = 0x19, //写入当前位置到ROM作为电机零点命令 + RMDRs485CMDOrder92 = 0x92, //读取多圈角度命令 + RMDRs485CMDOrder94 = 0x94, //读取单圈角度命令 + RMDRs485CMDOrder95 = 0x95, //清除电机角度命令(设置电机初始位置) + RMDRs485CMDOrder9A = 0x9A, //读取电机状态1和错误标志命令 + RMDRs485CMDOrder9B = 0x9B, //清除电机错误标志命令 + RMDRs485CMDOrder9C = 0x9C, //读取电机状态2命令 + RMDRs485CMDOrder9D = 0x9D, //读取电机状态3命令 + RMDRs485CMDOrder80 = 0x80, //电机关闭命令 + RMDRs485CMDOrder81 = 0x81, //电机停止命令 + RMDRs485CMDOrder88 = 0x88, //电机运行命令 + RMDRs485CMDOrderA1 = 0xA1, //转矩闭环控制命令 + RMDRs485CMDOrderA2 = 0xA2, //速度闭环控制命令 + RMDRs485CMDOrderA3 = 0xA3, //多圈位置闭环控制命令1 + RMDRs485CMDOrderA4 = 0xA4, //多圈位置闭环控制命令2 + RMDRs485CMDOrderA5 = 0xA5, //单圈位置闭环控制命令1 + RMDRs485CMDOrderA6 = 0xA6, //单圈位置闭环控制命令2 + RMDRs485CMDOrderA7 = 0xA7, //增量位置闭环控制命令1 + RMDRs485CMDOrderA8 = 0xA8, //增量位置闭环控制命令2 + RMDRs485CMDOrder12 = 0x12, //读取驱动和电机型号命令 + RMDRs485CMDOrderC2 = 0xC2, //读取多圈角度命令2 + RMDRs485CMDOrderD8 = 0xD8, //增量位置闭环控制命令2 } RMDMotorDriverRs485CMD; void motor_drive_uart_init(); void motor_drive_turn(int direction, int speed_level, double position); -double motor_drive_read_encoder(); \ No newline at end of file +double motor_drive_read_encoder(); +void motor_drive_set_packages(); +void motor_drive_set_packages_ctr(double position); +void motor_drive_hex_to_str(char *hex, int hex_len, char *str); \ No newline at end of file