diff --git a/main/main.c b/main/main.c index 1e8934a..3fa4a0f 100644 --- a/main/main.c +++ b/main/main.c @@ -64,7 +64,8 @@ void app_main(void) { // s_bluetooth_processer.auto_report_flag = false; // } - motor_drive_set_packages_ctr(26.34); + // motor_drive_set_packages_ctr(26.34); + ESP_LOGI(MAIN_TAG,"%lf",motor_drive_read_single_turn_angle()); // if ((motor_drive_read_encoder() == s_bluetooth_processer.position) && s_bluetooth_processer.motor_drive_turn_flag == true) { // ESP_LOGI("test", "info log ok\n"); diff --git a/main/motor_drive.c b/main/motor_drive.c index 53bbc17..9ac2ef3 100644 --- a/main/motor_drive.c +++ b/main/motor_drive.c @@ -72,7 +72,7 @@ double motor_drive_read_encoder() { encoder_data = buffer[5] + (buffer[6] << 8); // parse motor usart - return ((double)encoder_data / 100); + 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) { @@ -193,8 +193,27 @@ void motor_drive_buffer_cmd_parse(uint8_t *buffer) { void motor_drive_set_motor_size() {} double motor_drive_read_single_turn_angle() { - double turn_angle = 0.0; + uint16_t turn_angle_data = 0; + size_t encoder_buffer_size = 5; uint8_t buffer[5] = {0X3E, 0X94, 0X01, 0X00, 0XD3}; - return turn_angle; + uart_flush(uart_num); + uart_write_bytes(uart_num, buffer, 5); + + encoder_buffer_size = 0; + memset(buffer, 0, sizeof(uint8_t) * 5); + + // Wait receive + encoder_buffer_size = uart_read_bytes(uart_num, buffer, 8, 2000 / portTICK_RATE_MS); + if (encoder_buffer_size != 8 || buffer[0] != 0X3E) { + ESP_LOGW(MOTOR_DRIVE, "encoder size:%d,buffer[0] = 0X%x", encoder_buffer_size, buffer[0]); + 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); } \ No newline at end of file