You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
70 lines
1.9 KiB
70 lines
1.9 KiB
#include "motor_drive.h"
|
|
|
|
#include "driver/uart.h"
|
|
#include "esp_log.h"
|
|
|
|
#define MOTOR_DRIVE "MOTOR_DRIVE"
|
|
|
|
#define uart_num UART_NUM_1
|
|
#define tx_io_num 23
|
|
#define rx_io_num 22
|
|
#define buffer_size 128
|
|
#define uart_read_time_ms 200
|
|
#define MOTOR_ID 0X01
|
|
|
|
static motor_t *motor_init_structer;
|
|
|
|
void motor_init(motor_t *motor) {
|
|
motor_init_structer = motor;
|
|
uart_config_t uart_config = {
|
|
.baud_rate = 115200,
|
|
.data_bits = UART_DATA_8_BITS,
|
|
.parity = UART_PARITY_DISABLE,
|
|
.stop_bits = UART_STOP_BITS_1,
|
|
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
|
.source_clk = UART_SCLK_APB,
|
|
};
|
|
ESP_ERROR_CHECK(uart_driver_install(uart_num, buffer_size * 2, 0, 0, NULL, 0));
|
|
|
|
ESP_ERROR_CHECK(uart_param_config(uart_num, &uart_config));
|
|
|
|
ESP_ERROR_CHECK(uart_set_pin(uart_num, tx_io_num, rx_io_num, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
|
|
return;
|
|
}
|
|
void motor_set_zero_point() { return; }
|
|
/**
|
|
* @brief 返回当前电机所在位置0->360
|
|
*
|
|
* @return uint32_t
|
|
*/
|
|
uint32_t motor_get_position_degree() { return 0; }
|
|
/**
|
|
* @brief 电机转到多少度
|
|
*/
|
|
void motor_run_to_postion(int potion) { return; }
|
|
void motor_reg_event_cb(motor_on_event_t onevent) { return; }
|
|
|
|
double motor_drive_read_encoder() {
|
|
size_t encoder_buffer_size = 5;
|
|
uint8_t buffer[5] = {0X3E, 0X90, MOTOR_ID, 0X00, 0XCF};
|
|
uint16_t encoder_data = 0;
|
|
|
|
// Send cmd
|
|
uart_flush(uart_num);
|
|
uart_write_bytes(uart_num, (const char *)buffer, encoder_buffer_size);
|
|
|
|
encoder_buffer_size = 0;
|
|
memset(buffer, 0, sizeof(uint8_t) * 5);
|
|
|
|
// Wait receive
|
|
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]);
|
|
|
|
return -1.0;
|
|
}
|
|
|
|
encoder_data = buffer[5] + (buffer[6] << 8);
|
|
|
|
return ((double)encoder_data / 100.0);
|
|
}
|