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.
94 lines
2.5 KiB
94 lines
2.5 KiB
#include "servo_operation.h"
|
|
|
|
#include <stdbool.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
static UART_HandleTypeDef* m_uart;
|
|
|
|
static uint8_t tx_buf[PROCESS_MAX_LEN];
|
|
static uint8_t rx_buf[PROCESS_MAX_LEN];
|
|
static uint8_t tx_send_len;
|
|
|
|
static uint8_t cmd_type;
|
|
|
|
static uint8_t check_sum;
|
|
|
|
// 我需要一个能携带寄存器信息的变量
|
|
static reg_info_t reg_info;
|
|
|
|
|
|
|
|
/**
|
|
* @description: 舵机串口初始化
|
|
* @param {UART_HandleTypeDef*} uart
|
|
* @return {*}
|
|
*/
|
|
void servo_uart_init(UART_HandleTypeDef* uart) { m_uart = uart; }
|
|
|
|
/**
|
|
* @description: 写操作
|
|
* @param {servo_obj_t*} servo 舵机对象
|
|
* @return {*}
|
|
*/
|
|
bool Write_operete(uint8_t id, uint8_t cmd, param_t* param) {
|
|
reg_distinguish(param, ®_info);
|
|
|
|
if (cmd != SYC_W_DATA)
|
|
cmd_type = non_syn;
|
|
else
|
|
cmd_type = syn;
|
|
|
|
write_reg(id, cmd, param);
|
|
return true;
|
|
}
|
|
|
|
void Ping_operete(uint8_t id) {
|
|
cmd_basic_t* cmd_basic = (cmd_basic_t*)tx_buf;
|
|
cmd_basic->head = 0xffff;
|
|
cmd_basic->id = id;
|
|
cmd_basic->valid_len = 0x02;
|
|
cmd_basic->cmd = PING;
|
|
|
|
check_sum = check_sum_calculate(&tx_buf[2], NULL);
|
|
memcpy(&tx_buf[BASCI_LEN], &check_sum, 1);
|
|
tx_send_len = BASCI_LEN + 1; // 1 = check_sum
|
|
packet_process(m_uart, tx_buf, rx_buf, tx_send_len);
|
|
}
|
|
|
|
// 初始方案无法确定发送参数的长度，用了一个数组但是导致指令包过大，所以改了个方法
|
|
/**
|
|
* @description:
|
|
* @param {uint8_t} id
|
|
* @param {uint8_t} cmd
|
|
* @param {param_t*} param
|
|
* @return {*}
|
|
*/
|
|
void write_reg(uint8_t id, uint8_t cmd, param_t* param) {
|
|
uint8_t param_real_len = param->count * reg_info.data_len; // 参数个数*每个参数长度 = 实际参数长度
|
|
cmd_basic_t* cmd_basic = (cmd_basic_t*)tx_buf; // 解决发送数据不是数组问题
|
|
uint8_t* cmd_param = &tx_buf[5];
|
|
|
|
cmd_basic->head = 0xffff;
|
|
cmd_basic->id = id;
|
|
|
|
if (cmd_type) {
|
|
cmd_basic->valid_len = param_real_len + 3; // cmd + id + check_sum = 3
|
|
cmd_basic->cmd = cmd;
|
|
cmd_param[0] = param->addr;
|
|
// 判断参数数据长度
|
|
if (reg_info.data_len == byte) {
|
|
memcpy(&cmd_param[1], param->data, param->count);
|
|
} else if (reg_info.data_len == word) {
|
|
// 高低位交换，传入的寄存器地址不需要交换
|
|
high_low_exchange(param, &cmd_param[1], ®_info);
|
|
}
|
|
}
|
|
// else 同步和非同步无法合并为一个函数
|
|
check_sum = check_sum_calculate(&tx_buf[2], param);
|
|
// 将checksum放入指令包末尾
|
|
memcpy(&tx_buf[BASCI_LEN + param_real_len + 1], &check_sum, 1); // tx_buff[5] == addr 1 = addr
|
|
|
|
tx_send_len = BASCI_LEN + param_real_len + 2; // 2 = addr + check_sum
|
|
packet_process(m_uart, tx_buf,rx_buf, tx_send_len);
|
|
}
|