diff --git a/main/ble_parse_data.c b/main/ble_parse_data.c index d0f3088..df85588 100644 --- a/main/ble_parse_data.c +++ b/main/ble_parse_data.c @@ -39,7 +39,8 @@ void bluetooth_gatts_try_process_data() { encoder_befor_num = motor_drive_read_encoder(); if (encoder_befor_num >= 0) { - if (motor_drive_set_packages_ctr(55.22) == 0) { + if (motor_drive_set_packages_ctr(parse_bluetooth_processer->position) == 0) { + ets_delay_us(50000); if (encoder_befor_num == motor_drive_read_encoder()) { ESP_LOGW(BLE_PARSE_DATA_TAG, "motor no turning"); } else { diff --git a/main/main.c b/main/main.c index 8cbed5f..228c54f 100644 --- a/main/main.c +++ b/main/main.c @@ -62,6 +62,8 @@ void app_main(void) { // ESP_LOGI(MAIN_TAG, "encoder :%.2lf", encoder); // s_bluetooth_processer.auto_report_flag = false; // } + + // motor_drive_set_motor_to_angle(0,35.32,5); } return; diff --git a/main/motor_drive.c b/main/motor_drive.c index 7e9229c..8fa1813 100644 --- a/main/motor_drive.c +++ b/main/motor_drive.c @@ -10,7 +10,8 @@ #define tx_io_num 1 #define rx_io_num 2 #define buffer_size 128 -#define uart_read_time_ms 1 +#define uart_read_time_ms 200 +#define MOTOR_ID 0X01 void motor_drive_uart_init() { uart_config_t uart_config = { @@ -44,7 +45,7 @@ void motor_drive_turn(int direction, int speed_level, double position) { double motor_drive_read_encoder() { size_t encoder_buffer_size = 5; - uint8_t buffer[5] = {0X3E, 0X90, 0X01, 0X00, 0XCF}; + uint8_t buffer[5] = {0X3E, 0X90, MOTOR_ID, 0X00, 0XCF}; uint16_t encoder_data = 0; char *notify_err = "read encoder error"; @@ -90,7 +91,7 @@ uint8_t motor_drive_set_packages_ctr(double position) { uint8_t position_remainder = 0; uint8_t position_buffer_size = 5; //从第五位开始(低位) uint8_t checksum = 0; - uint8_t buffer[10] = {0x3E, 0XA7, 0X01, 0X04, 0XEA, 0X00, 0X00, 0X00, 0X00, 0X00}; + uint8_t buffer[10] = {0x3E, 0XA7, MOTOR_ID, 0X04, 0XEA, 0X00, 0X00, 0X00, 0X00, 0X00}; char *notify_err = "set size error"; position_int = position * 100; @@ -185,7 +186,7 @@ void motor_drive_buffer_cmd_parse(uint8_t *buffer) { return; } - if (buffer[0] != 0X3E || buffer[2] != 0X01) { + if (buffer[0] != 0X3E || buffer[2] != MOTOR_ID) { ESP_LOGW(MOTOR_DRIVE, "cmd parse buffer error"); return; } @@ -198,7 +199,7 @@ void motor_drive_buffer_cmd_parse(uint8_t *buffer) { 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}; + uint8_t buffer[5] = {0X3E, 0X19, MOTOR_ID, 0X00, 0X58}; char *notify_err = "Set current size err"; uart_flush(uart_num); uart_write_bytes(uart_num, buffer, 5); @@ -220,7 +221,7 @@ u_int8_t motor_drive_set_motor_current_size() { double motor_drive_read_single_turn_angle() { uint16_t turn_angle_data = 0; size_t turn_angle_buffer_size = 5; - uint8_t buffer[5] = {0X3E, 0X94, 0X01, 0X00, 0XD3}; + uint8_t buffer[5] = {0X3E, 0X94, MOTOR_ID, 0X00, 0XD3}; char *notify_err = "Turn angle size err"; uart_flush(uart_num); @@ -243,4 +244,72 @@ double motor_drive_read_single_turn_angle() { turn_angle_data = buffer[5] + (buffer[6] << 8); return ((double)turn_angle_data / 100.0); -} \ No newline at end of file +} + +// void motor_drive_set_motor_to_angle(uint8_t rotation_direction, double position, uint8_t speed_level) { +// int position_int = 0; +// uint8_t position_remainder = 0; +// uint8_t position_buffer_size = 6; //从第五位开始(低位) +// uint8_t checksum = 0; +// uint32_t limit_speed = 0; +// uint8_t buffer[14] = {0x3E, 0XA6, MOTOR_ID, 0X08, 0XED, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00}; +// char *notify_err = "set size error"; + +// if ((rotation_direction == 0X00) || (rotation_direction == 0X01)) { +// buffer[5] = rotation_direction; +// checksum += rotation_direction; +// } + +// position_int = position * 100; +// position_int = position_int % 36000; + +// if (position_int != 0) { +// while ((position_int / 0XFF) > 0) { +// position_remainder = position_int & 0XFF; +// buffer[position_buffer_size] = position_remainder; +// position_buffer_size += 1; +// position_int = position_int >> 8; +// checksum += position_remainder; +// } +// buffer[position_buffer_size] = position_int; +// checksum += position_int; +// } + +// if (speed_level <= 9) { +// limit_speed = speed_level * 360; +// position_buffer_size = 9; +// } + +// while (limit_speed != 0) { +// while ((limit_speed / 0XFF) > 0) { +// position_remainder = limit_speed & 0XFF; +// buffer[position_buffer_size] = position_remainder; +// position_buffer_size += 1; +// limit_speed = limit_speed >> 8; +// checksum += position_remainder; +// } +// buffer[position_buffer_size] = limit_speed; +// checksum += limit_speed; +// } + +// checksum = checksum & 0XFF; +// buffer[13] = checksum; + +// Send cmd +// uart_flush(uart_num); +// uart_write_bytes(uart_num, buffer, 14); + +// position_buffer_size = 0; +// memset(buffer, 0, sizeof(uint8_t) * 14); + +// Wait uart receive,if time out return error and output log +// 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)); +// } + +// Parse receive +// motor_drive_buffer_cmd_parse(buffer); +// return; +// } \ No newline at end of file diff --git a/main/motor_drive.h b/main/motor_drive.h index e237fe3..1014fe4 100644 --- a/main/motor_drive.h +++ b/main/motor_drive.h @@ -45,4 +45,5 @@ 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); u_int8_t motor_drive_set_motor_current_size(); -double motor_drive_read_single_turn_angle(); \ No newline at end of file +double motor_drive_read_single_turn_angle(); +// void motor_drive_set_motor_to_angle(uint8_t rotation_direction, double position, uint8_t speed_level); \ No newline at end of file