diff --git a/main/ble_parse_data.c b/main/ble_parse_data.c index e336810..0f99b34 100644 --- a/main/ble_parse_data.c +++ b/main/ble_parse_data.c @@ -7,6 +7,8 @@ #define cmd_length_set_position 5 #define cmd_length_get_status 2 +static double encoder_befor_num; + static bluetooth_processer_t *parse_bluetooth_processer; uint8_t bluetooth_rx_buffer_len = 0; @@ -34,9 +36,19 @@ void bluetooth_gatts_try_process_data() { if (strcmp(parse_bluetooth_processer->order, set_position) == 0) { parse_bluetooth_processer->auto_report_flag = true; ESP_LOGI(BLE_PARSE_DATA_TAG, set_position); - motor_drive_set_packages_ctr(352.68); - // motor_cmd_set_position(parse_bluetooth_processer->speedLevel, parse_bluetooth_processer->position, parse_bluetooth_processer->direction); - receipt_json_set_position(); + + encoder_befor_num = motor_drive_read_encoder(); + if (encoder_befor_num >= 0) { + if (motor_drive_set_packages_ctr(55.22) == 0) { + if (encoder_befor_num == motor_drive_read_encoder()) { + ESP_LOGW(BLE_PARSE_DATA_TAG, "motor no turning"); + } else { + ESP_LOGI(BLE_PARSE_DATA_TAG, "motor turning"); + } + } + } + + // receipt_json_set_position(); } if (strcmp(parse_bluetooth_processer->order, get_status) == 0) { ESP_LOGI(BLE_PARSE_DATA_TAG, get_status); diff --git a/main/main.c b/main/main.c index b9faf22..8cbed5f 100644 --- a/main/main.c +++ b/main/main.c @@ -53,25 +53,13 @@ void app_main(void) { ble_spp_server_demo_app_main(&s_bluetooth_processer); timer_group_init(TIMER_GROUP_0, TIMER_0, false, timer_group0_interval_num, timer_interval_ms); motor_drive_uart_init(); - // ble_uart_init(UART_NUM_1, 1, 2); - // char* test_str = "This is a test string.\n"; - // double encoder = 0.0; 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; - // } - motor_drive_read_encoder(); - // 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 (s_bluetooth_processer.auto_report_flag) { // encoder = motor_drive_read_encoder(); - // ESP_LOGI(MAIN_TAG, "encoder :%.2lf",encoder); + // ESP_LOGI(MAIN_TAG, "encoder :%.2lf", encoder); // s_bluetooth_processer.auto_report_flag = false; // } } diff --git a/main/motor_drive.c b/main/motor_drive.c index d3b6293..34925c1 100644 --- a/main/motor_drive.c +++ b/main/motor_drive.c @@ -10,6 +10,7 @@ #define tx_io_num 1 #define rx_io_num 2 #define buffer_size 128 +#define uart_read_time_ms 2000 void motor_drive_uart_init() { uart_config_t uart_config = { @@ -62,15 +63,15 @@ double motor_drive_read_encoder() { memset(buffer, 0, sizeof(uint8_t) * 5); // Wait receive - encoder_buffer_size = uart_read_bytes(uart_num, buffer, 12, 2000 / portTICK_RATE_MS); + 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; + return -1.0; } // Parse receive - motor_drive_buffer_cmd_parse(buffer); + // motor_drive_buffer_cmd_parse(buffer); encoder_data = buffer[5] + (buffer[6] << 8); @@ -84,7 +85,7 @@ void motor_drive_set_packages_data_max64bit(uint8_t cmd, uint8_t buffer_data_siz motor_drive_buffer_cmd_generate(buffer, cmd, buffer_data_size, buffer_data); } -void motor_drive_set_packages_ctr(double position) { +uint8_t motor_drive_set_packages_ctr(double position) { int position_int = 0; uint8_t position_remainder = 0; uint8_t position_buffer_size = 5; //从第五位开始(低位) @@ -116,15 +117,16 @@ void motor_drive_set_packages_ctr(double position) { 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, 2000 / portTICK_RATE_MS); + 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; + return 1; } // Parse receive // motor_drive_buffer_cmd_parse(buffer); + return 0; } void motor_drive_hex_to_str(const char *hex, int hex_len, char *str) { @@ -169,15 +171,15 @@ size_t motor_drive_buffer_cmd_generate(uint8_t *buffer, uint8_t cmd, uint8_t buf 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); + // 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]); + // 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; @@ -188,10 +190,10 @@ void motor_drive_buffer_cmd_parse(uint8_t *buffer) { 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]); - } + // 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() { @@ -205,7 +207,7 @@ u_int8_t motor_drive_set_motor_current_size() { memset(buffer, 0, sizeof(uint8_t) * 5); // Wait receive - set_current_buffer_size = uart_read_bytes(uart_num, buffer, 5, 2000 / portTICK_RATE_MS); + 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)); @@ -228,7 +230,7 @@ double motor_drive_read_single_turn_angle() { memset(buffer, 0, sizeof(uint8_t) * 5); // Wait receive - turn_angle_buffer_size = uart_read_bytes(uart_num, buffer, 8, 2000 / portTICK_RATE_MS); + 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)); @@ -236,7 +238,7 @@ double motor_drive_read_single_turn_angle() { } // Parse receive - motor_drive_buffer_cmd_parse(buffer); + // motor_drive_buffer_cmd_parse(buffer); turn_angle_data = buffer[5] + (buffer[6] << 8); diff --git a/main/motor_drive.h b/main/motor_drive.h index 97a40e3..e237fe3 100644 --- a/main/motor_drive.h +++ b/main/motor_drive.h @@ -40,7 +40,7 @@ void motor_drive_uart_init(); void motor_drive_turn(int direction, int speed_level, double position); double motor_drive_read_encoder(); void motor_drive_set_packages_data_max64bit(uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data); -void motor_drive_set_packages_ctr(double position); +uint8_t motor_drive_set_packages_ctr(double position); void motor_drive_hex_to_str(const char *hex, int hex_len, char *str); size_t motor_drive_buffer_cmd_generate(uint8_t *buffer, uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data); void motor_drive_buffer_cmd_parse(uint8_t *buffer);