From bd975e31a1b81dd4fbb202f7317394ba7bcded91 Mon Sep 17 00:00:00 2001 From: zwsd Date: Wed, 3 Aug 2022 11:59:48 +0800 Subject: [PATCH] =?UTF-8?q?=E8=BD=AC=E5=88=B0=E7=9B=AE=E6=A0=87=E8=A7=92?= =?UTF-8?q?=E5=BA=A6=E4=B8=BB=E5=8A=A8=E4=B8=8A=E6=8A=A5=EF=BC=8C=E8=BF=99?= =?UTF-8?q?=E6=9C=89=E4=B8=A4=E4=B8=AA=E9=97=AE=E9=A2=98=EF=BC=8C1.index?= =?UTF-8?q?=E4=B8=8D=E6=98=AF=E4=B8=8A=E4=BC=A0=E6=97=B6=E7=9A=84=EF=BC=8C?= =?UTF-8?q?2.notify=E5=8F=91=E9=80=81=E6=95=B4=E4=B8=AAbuffer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 3 +- main/main.c | 10 +++--- main/motor_drive.c | 89 +++++++++++++++++++++++++++++++++++++++++++++++++-- main/motor_drive.h | 2 +- 4 files changed, 96 insertions(+), 8 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 4641089..a6af20f 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -38,6 +38,7 @@ "freertos.h": "c", "bitset": "c", "chrono": "c", - "algorithm": "c" + "algorithm": "c", + "port.h": "c" }, } \ No newline at end of file diff --git a/main/main.c b/main/main.c index 96bbf24..142c912 100644 --- a/main/main.c +++ b/main/main.c @@ -80,9 +80,14 @@ void blerxcb(uint8_t *rxmessage, size_t rxsize) { void motor_on_event(motor_event_t event) { //打算使用sprintf if (event == kRunToPosition) { + ESP_LOGI(MAIN_LOG_TAG, "kRunToPosition ok!"); + + uint8_t buffer[128] = {0}; //组包 //上报 //{ "order": "deviceStatusReport", "index": 0, "deviceState": "running", "position":13.9, "deviceException": 0, "deviceExceptionInfo": "", } + sprintf((char *)buffer, "{ \"order\": \"receipt\", \"code\": 0, \"info\": \"success\", \"index\": %d }", 0); + bleuart_notify_send(buffer,sizeof(buffer)); } } @@ -93,11 +98,8 @@ void app_main(void) { motor_init(&ble_uart_motor_structer); motor_reg_event_cb(motor_on_event); - uint8_t test[] = {"peiluoxisibahhh,finny noob !lol"}; - while (true) { bleuart_schedule(); - // motor_module_schedule(); - bleuart_notify_send(test,sizeof(test)); + motor_module_schedule(); } } diff --git a/main/motor_drive.c b/main/motor_drive.c index 68d9385..e5200ae 100644 --- a/main/motor_drive.c +++ b/main/motor_drive.c @@ -2,6 +2,7 @@ #include "driver/uart.h" #include "esp_log.h" +#include "port.h" #define MOTOR_DRIVE "MOTOR_DRIVE" @@ -12,7 +13,13 @@ #define uart_read_time_ms 200 #define MOTOR_ID 0X01 +#define angular_error 1.0 //角度误差 + static motor_t *motor_init_structer; +static motor_on_event_t s_onevent; + +static bool notify_send_flag = false; +static double motor_position; /** * @brief 电机初始化 @@ -69,7 +76,33 @@ void motor_set_zero_point() { * * @return uint32_t */ -uint32_t motor_get_position_degree() { return 0; } +double motor_get_position_degree() { + 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); +} /** * @brief 电机转到多少度 @@ -142,8 +175,12 @@ void motor_run_to_postion(int rotation_direction, double position, int speed_lev 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; } + notify_send_flag = true; + motor_position = position; + // Parse receive // motor_drive_buffer_cmd_parse(buffer); return; @@ -154,7 +191,10 @@ void motor_run_to_postion(int rotation_direction, double position, int speed_lev * * @param onevent 注册的回调函数 */ -void motor_reg_event_cb(motor_on_event_t onevent) { return; } +void motor_reg_event_cb(motor_on_event_t onevent) { + s_onevent = onevent; + return; +} /** * @brief 读取编码器 @@ -258,4 +298,49 @@ static uint8_t motor_drive_set_packages_ctr(double position, int direction) { // Parse receive // motor_drive_buffer_cmd_parse(buffer); return 0; +} + +/** + * @brief 电机模块调度(验证电机是否到达指定位置，然后主动上报数据) + * + */ +void motor_module_schedule() { + static uint32_t time = 0; + //读取角度是否到达想要角度，到达上报(每隔500ms读取一次) + if ((notify_send_flag == true) && (port_haspassedms(time) > 500)) { + double now_position_degree = motor_get_position_degree(); + double maximum = 0.0; + double minimum = 0.0; + + if (motor_position == 0){ + motor_position = 360; + } + + if ((motor_position + angular_error) > 360.0) { + maximum = (motor_position + angular_error) - 360.0; + } else { + maximum = motor_position + angular_error; + } + + if ((motor_position - angular_error) < 0.0) { + minimum = (motor_position - angular_error) + 360.0; + } else { + minimum = motor_position - angular_error; + } + + if (maximum < minimum) { + maximum = maximum + 360; + } + + if ((now_position_degree >= minimum) && (now_position_degree <= maximum)) { //读取角度是否到达 + s_onevent(kRunToPosition); //组包，主动上报 + notify_send_flag = false; + } + + // ESP_LOGI("=======", "now:%lf,run to:%lf,minimum = %lf,maximum = %lf", now_position_degree, motor_position, minimum, maximum); + + time = port_get_ticket(); + } + + return; } \ No newline at end of file diff --git a/main/motor_drive.h b/main/motor_drive.h index dcfcbc2..75b94d2 100644 --- a/main/motor_drive.h +++ b/main/motor_drive.h @@ -12,7 +12,7 @@ typedef void (*motor_on_event_t)(motor_event_t event); void motor_init(motor_t* motor); void motor_set_zero_point(); -uint32_t motor_get_position_degree(); +double motor_get_position_degree(); void motor_run_to_postion(int rotation_direction, double position, int speed_level); void motor_reg_event_cb(motor_on_event_t onevent);