diff --git a/main/main.c b/main/main.c index 17816fd..6bc950c 100644 --- a/main/main.c +++ b/main/main.c @@ -68,7 +68,8 @@ void app_main(void) { // s_bluetooth_processer.motor_drive_turn_flag = false; // } - motor_drive_set_packages_ctr(35.68); + // motor_drive_set_packages_ctr(35.68); + motor_drive_set_packages_data_max64bit(0x34,4,0X76482534); // 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 f0e28d5..71215d9 100644 --- a/main/motor_drive.c +++ b/main/motor_drive.c @@ -42,7 +42,11 @@ void motor_drive_turn(int direction, int speed_level, double position) { double motor_drive_read_encoder() { return 0.0; } -void motor_drive_set_packages() {} +void motor_drive_set_packages_data_max64bit(uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data) { + uint8_t buffer[20] = {0}; + + motor_drive_buffer_init(buffer, cmd, buffer_data_size, buffer_data); +} void motor_drive_set_packages_ctr(double position) { int position_int = 0; @@ -84,4 +88,33 @@ void motor_drive_hex_to_str(char *hex, int hex_len, char *str) { pos += 2; } +} + +void motor_drive_buffer_init(uint8_t *buffer, uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data) { + uint8_t i = 0; + uint8_t checksum = 0; + uint8_t buffer_data_uint8 = 0; + uint8_t strbuffer[20] = {'\0'}; + if (buffer == NULL) { + ESP_LOGW(MOTOR_DRIVE, "buffer nil ,init error"); + return; + } + buffer[0] = 0X3E; + buffer[1] = cmd; + buffer[2] = 0X1; + buffer[3] = buffer_data_size; + buffer[4] = (0X3E + cmd + 0X1 + buffer_data_size) % 255; + + if (buffer_data_size > 0) { + for (i = 0; i < buffer_data_size; i++) { + buffer_data_uint8 = buffer_data; + buffer_data = buffer_data >> 8; + buffer[5 + i] = buffer_data_uint8; + checksum += buffer_data_uint8; + } + buffer[5 + buffer_data_size] = (checksum % 255); + } + + motor_drive_hex_to_str((char *)buffer, (5 + buffer_data_size + 1), (char *)strbuffer); + ESP_LOGI(MOTOR_DRIVE, "%s", strbuffer); } \ No newline at end of file diff --git a/main/motor_drive.h b/main/motor_drive.h index c674aa2..9d25683 100644 --- a/main/motor_drive.h +++ b/main/motor_drive.h @@ -39,6 +39,7 @@ typedef enum { 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(); +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); -void motor_drive_hex_to_str(char *hex, int hex_len, char *str); \ No newline at end of file +void motor_drive_hex_to_str(char *hex, int hex_len, char *str); +void motor_drive_buffer_init(uint8_t *buffer, uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data); \ No newline at end of file