diff --git a/main/main.c b/main/main.c index 3fa4a0f..36de35c 100644 --- a/main/main.c +++ b/main/main.c @@ -65,7 +65,7 @@ void app_main(void) { // } // motor_drive_set_packages_ctr(26.34); - ESP_LOGI(MAIN_TAG,"%lf",motor_drive_read_single_turn_angle()); + ESP_LOGI(MAIN_TAG,"%d",motor_drive_set_motor_current_size()); // 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 9ac2ef3..b462c17 100644 --- a/main/motor_drive.c +++ b/main/motor_drive.c @@ -190,23 +190,41 @@ void motor_drive_buffer_cmd_parse(uint8_t *buffer) { } } -void motor_drive_set_motor_size() {} +u_int8_t motor_drive_set_motor_current_size() { + size_t set_current_buffer_size = 5; + uint8_t buffer[5] = {0X3E, 0X19, 0X01, 0X00, 0X58}; + + 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, 2000 / 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]); + return 1; + } + + return 0; +} double motor_drive_read_single_turn_angle() { uint16_t turn_angle_data = 0; - size_t encoder_buffer_size = 5; + size_t turn_angle_buffer_size = 5; uint8_t buffer[5] = {0X3E, 0X94, 0X01, 0X00, 0XD3}; uart_flush(uart_num); uart_write_bytes(uart_num, buffer, 5); - encoder_buffer_size = 0; + turn_angle_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]); + turn_angle_buffer_size = uart_read_bytes(uart_num, buffer, 8, 2000 / 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]); return -1; } @@ -215,5 +233,5 @@ double motor_drive_read_single_turn_angle() { turn_angle_data = buffer[5] + (buffer[6] << 8); - return ((double)turn_angle_data/100.0); + return ((double)turn_angle_data / 100.0); } \ No newline at end of file diff --git a/main/motor_drive.h b/main/motor_drive.h index cc322a1..97a40e3 100644 --- a/main/motor_drive.h +++ b/main/motor_drive.h @@ -44,5 +44,5 @@ void 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); -void motor_drive_set_motor_size(); +u_int8_t motor_drive_set_motor_current_size(); double motor_drive_read_single_turn_angle(); \ No newline at end of file