|
|
|
@ -1,153 +1,165 @@ |
|
|
|
|
|
|
|
|
|
|
|
#include "servo_package_process.h" |
|
|
|
|
|
|
|
#include <stdio.h> |
|
|
|
#include <string.h> |
|
|
|
|
|
|
|
#include "tick.h" |
|
|
|
#include "servo_driver.h" |
|
|
|
#include "servo_reg_map.h" |
|
|
|
static UART_HandleTypeDef* m_uart; |
|
|
|
static uint8_t rx_buf[PROCESS_MAX_LEN]; |
|
|
|
static uint8_t rx_size; |
|
|
|
static uint8_t tx_buf[PROCESS_MAX_LEN]; |
|
|
|
static uint8_t tx_send_len; |
|
|
|
static bool Idle_Flag = false; |
|
|
|
|
|
|
|
static uint8_t cmd_type; |
|
|
|
static cmd_frame_t cmd; |
|
|
|
static uint8_t cmd_packet[PACKET_MAX_LEN]; |
|
|
|
|
|
|
|
static uint8_t reg_map[k_reg_max_map] = { |
|
|
|
kRegFirmwareMainVersion, |
|
|
|
kRegFirmwareSubVersion, |
|
|
|
kRegServoMainVersion, |
|
|
|
kRegServoSubVersion, |
|
|
|
kRegServoId, |
|
|
|
kRegServoBaudRate, |
|
|
|
kRegServoDelay, |
|
|
|
kRegServoAckLevel, |
|
|
|
kRegServoMinAngle, |
|
|
|
kRegServoMaxAngle, |
|
|
|
kRegServoMaxTemp, |
|
|
|
kRegServoMaxVoltage, |
|
|
|
kRegServoMinVoltage, |
|
|
|
kRegServoMaxTorque, |
|
|
|
kRegServoPhase, |
|
|
|
kRegServoUnloadCondition, |
|
|
|
kRegServoLedAlarmCondition, |
|
|
|
kRegServoP, |
|
|
|
kRegServoD, |
|
|
|
kRegServoI, |
|
|
|
kRegServoMinStart, |
|
|
|
kRegServoCwDeadZone, |
|
|
|
kRegServoCcwDeadZone, |
|
|
|
kRegServoProtectCurrent, |
|
|
|
kRegServoAngleResolution, |
|
|
|
kRegServoCalibration, |
|
|
|
kRegServoRunMode, |
|
|
|
kRegServoProtectTorque, |
|
|
|
kRegServoProtectTime, |
|
|
|
kRegServoOverloadTorque, |
|
|
|
kRegServoSpeedP, |
|
|
|
kRegServoOverloadTime, |
|
|
|
kRegServoSpeedI, |
|
|
|
kRegServoTorqueSwitch, |
|
|
|
kRegServoAcc, |
|
|
|
kRegServoTargetPos, |
|
|
|
kRegServoRunTime, |
|
|
|
kRegServoRunSpeed, |
|
|
|
kRegServoTorqueLimit, |
|
|
|
kRegServoLockFlag, |
|
|
|
kRegServoCurrentPos, |
|
|
|
kRegServoCurrentSpeed, |
|
|
|
kRegServoCurrentLoad, |
|
|
|
kRegServoCurrentVoltage, |
|
|
|
kRegServoCurrentTemp, |
|
|
|
kRegServoAsyncWriteFlag, |
|
|
|
kRegServoStatus, |
|
|
|
kRegServoMoveFlag, |
|
|
|
kRegServoCurrentCurrent, |
|
|
|
kRegServoCheckSpeed, |
|
|
|
kRegServoDTime, |
|
|
|
kRegServoSpeedUnit, |
|
|
|
kRegServoMinSpeedLimit, |
|
|
|
kRegServoMaxSpeedLimit, |
|
|
|
kRegServoAccLimit, |
|
|
|
kRegServoAccMultiple, |
|
|
|
}; |
|
|
|
static uint8_t cmd_type; |
|
|
|
// 我需要一个能携带寄存器信息的变量 |
|
|
|
static reg_info_t reg_info; |
|
|
|
|
|
|
|
void SET_IDEL_FLAG(void) { Idle_Flag = true; } |
|
|
|
void CLEAR_IDEL_FLAG(void) { Idle_Flag = false; } |
|
|
|
|
|
|
|
/** |
|
|
|
* @description: 舵机串口初始化 |
|
|
|
* @param {UART_HandleTypeDef*} uart |
|
|
|
* @return {*} |
|
|
|
*/ |
|
|
|
void servo_uart_init(UART_HandleTypeDef* uart) { m_uart = uart; } |
|
|
|
|
|
|
|
/** |
|
|
|
* @description: 判断字符串长度 |
|
|
|
* @param {uint8_t} reg_addr |
|
|
|
* @return {*} |
|
|
|
*///原先由于寄存器不连贯，导致进行判断时变得繁琐 |
|
|
|
static uint8_t estimate_data_len(uint8_t reg_addr) { |
|
|
|
if (reg_addr > k_reg_max_map) return error; |
|
|
|
|
|
|
|
switch (reg_addr) { |
|
|
|
case kRegServoMinAngle_map: |
|
|
|
case kRegServoMaxAngle_map: |
|
|
|
case kRegServoMaxTorque_map: |
|
|
|
case kRegServoProtectCurrent_map: |
|
|
|
case kRegServoCalibration_map: |
|
|
|
case kRegServoTargetPos_map: |
|
|
|
case kRegServoRunTime_map: |
|
|
|
case kRegServoRunSpeed_map: |
|
|
|
case kRegServoTorqueLimit_map: |
|
|
|
case kRegServoCurrentPos_map: |
|
|
|
case kRegServoCurrentSpeed_map: |
|
|
|
case kRegServoCurrentLoad_map: |
|
|
|
case kRegServoCurrentCurrent_map: |
|
|
|
return word; |
|
|
|
*/ |
|
|
|
// 特殊的寄存器只是比原先普通的寄存器多携带 了一些信息 |
|
|
|
// 我只需要判断出特殊的寄存器，然后给他们加上多于普通寄存器的信息就行了 |
|
|
|
// 普通寄存器默认信息：输入数据是一个字节 不用特殊位处理 |
|
|
|
|
|
|
|
void reg_distinguish(param_t* param) { |
|
|
|
reg_info.addr = (uint8_t)param->addr; |
|
|
|
|
|
|
|
switch (reg_info.addr) { |
|
|
|
case kRegServoCalibration: |
|
|
|
reg_info.data_len = word; |
|
|
|
reg_info.special_bit = BT_11; |
|
|
|
reg_info.sign_ed = 1; |
|
|
|
break; |
|
|
|
case kRegServoRunTime: |
|
|
|
reg_info.data_len = word; |
|
|
|
reg_info.special_bit = BT_10; |
|
|
|
reg_info.sign_ed = 1; |
|
|
|
break; |
|
|
|
case kRegServoCurrentPos: |
|
|
|
case kRegServoCurrentSpeed: |
|
|
|
reg_info.data_len = word; |
|
|
|
reg_info.special_bit = BT_15; |
|
|
|
reg_info.sign_ed = 1; |
|
|
|
break; |
|
|
|
case kRegServoTargetPos: |
|
|
|
case kRegServoMinAngle: |
|
|
|
case kRegServoMaxAngle: |
|
|
|
case kRegServoMaxTorque: |
|
|
|
case kRegServoProtectCurrent: |
|
|
|
case kRegServoRunSpeed: |
|
|
|
case kRegServoTorqueLimit: |
|
|
|
case kRegServoCurrentLoad: |
|
|
|
case kRegServoCurrentCurrent: |
|
|
|
reg_info.data_len = word; |
|
|
|
reg_info.special_bit = 0; |
|
|
|
reg_info.sign_ed = 0; |
|
|
|
break; |
|
|
|
default: |
|
|
|
return byte; |
|
|
|
reg_info.data_len = byte; |
|
|
|
reg_info.special_bit = 0; |
|
|
|
reg_info.sign_ed = 0; |
|
|
|
break; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
void send_cmd_packet(cmd_frame_t* cmd) { |
|
|
|
memcpy(cmd_packet, cmd->head, sizeof(cmd->head)); |
|
|
|
cmd_packet[2] = cmd->id; |
|
|
|
cmd_packet[3] = cmd->len; |
|
|
|
memcpy(&cmd_packet[4], cmd->param, strlen((const char*)cmd->param)); |
|
|
|
static uint8_t check_sum_calculate(uint8_t id, uint8_t len, uint8_t cmd, param_t* param) { |
|
|
|
// Check Sum = ~ (ID + Length + Instruction + Parameter1 + ... Parameter N) |
|
|
|
uint8_t sum = 0; |
|
|
|
sum = id + len + cmd + param->addr; |
|
|
|
for (int16_t i = 0; i < param->count; i++) { |
|
|
|
sum += (uint8_t)param->data[i]; |
|
|
|
} |
|
|
|
|
|
|
|
HAL_UART_Transmit(m_uart, cmd_packet, sizeof(cmd), 10); |
|
|
|
// printf("cmd_frame_packet:%02X %02X %02X %02X %02X %02X %02X\n", cmd_packet[0], cmd_packet[1], cmd_packet[2], cmd_packet[3], cmd_packet[4], cmd_packet[5], cmd_packet[6]); |
|
|
|
return ~(sum); |
|
|
|
} |
|
|
|
|
|
|
|
static void high_low_exchange(param_t* param, uint8_t* packet) { |
|
|
|
static int16_t trans_buf; |
|
|
|
|
|
|
|
for (uint8_t i = 0; i < param->count; i++) { |
|
|
|
trans_buf = (param->data[i] | (reg_info.sign_ed << reg_info.special_bit)) >> 8; |
|
|
|
trans_buf |= param->data[i] << 8; |
|
|
|
memcpy(packet + (uint8_t)(i * 2), &trans_buf, sizeof(trans_buf)); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
void get_rx_size(uint16_t Size) {rx_size = Size - __HAL_DMA_GET_COUNTER(m_uart->hdmarx);} |
|
|
|
|
|
|
|
void packet_process(uint8_t len) { |
|
|
|
cmd_receive_t* receive = (cmd_receive_t*)rx_buf; |
|
|
|
uint32_t lasttime = HAL_GetTick(); |
|
|
|
|
|
|
|
HAL_UARTEx_ReceiveToIdle_DMA(m_uart, rx_buf, sizeof(rx_buf)); |
|
|
|
HAL_UART_Transmit(m_uart, tx_buf, len, 10); |
|
|
|
// HAL_UART_Receive_DMA(); |
|
|
|
|
|
|
|
while (!Idle_Flag) { |
|
|
|
// 没接收到数据进入等待 |
|
|
|
if (pass_time(lasttime) > OVERTIME) { |
|
|
|
printf("uart3 receive error\n"); |
|
|
|
break; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
if(receive->status == 0x00 && Idle_Flag){ |
|
|
|
printf("servo normal work\n"); |
|
|
|
CLEAR_IDEL_FLAG(); |
|
|
|
} |
|
|
|
|
|
|
|
printf("tx:%02X %02X %02X %02X %02X %02X %02X %02X %02X\n", tx_buf[0], tx_buf[1], tx_buf[2], tx_buf[3], tx_buf[4], tx_buf[5], tx_buf[6], tx_buf[7], tx_buf[8]); |
|
|
|
printf("tx_send_len: %02X\n", len); |
|
|
|
} |
|
|
|
|
|
|
|
// 初始方案无法确定发送参数的长度，用了一个数组但是导致指令包过大，所以改了个方法 |
|
|
|
/** |
|
|
|
* @description: 写寄存器(只实现了单个数据的写入 不支持多数据) |
|
|
|
* @param {servo_obj_t*} servo 舵机对象 |
|
|
|
* @param {uint8_t} param_len 数据写入寄存器中的长度 |
|
|
|
* @description: |
|
|
|
* @param {uint8_t} id |
|
|
|
* @param {uint8_t} cmd |
|
|
|
* @param {param_t*} param |
|
|
|
* @return {*} |
|
|
|
*/ |
|
|
|
void write_reg(servo_obj_t* servo, uint8_t param_len) { |
|
|
|
cmd.head[0] = 0xff; |
|
|
|
cmd.head[1] = 0xff; |
|
|
|
cmd.id = servo->id; |
|
|
|
if (cmd_type) { |
|
|
|
cmd.len = param_len + 3; |
|
|
|
cmd.param[0] = reg_map[servo->reg]; |
|
|
|
cmd.cmd = servo->cmd; |
|
|
|
void write_reg(uint8_t id, uint8_t cmd, param_t* param) { |
|
|
|
uint8_t check_sum = 0; |
|
|
|
uint8_t cmd_packet_len = 0; |
|
|
|
cmd_frame_t* cmd_packet = (cmd_frame_t*)tx_buf; // 解决发送数据不是数组问题 |
|
|
|
|
|
|
|
cmd_packet->head = 0xffff; |
|
|
|
cmd_packet->id = id; |
|
|
|
|
|
|
|
if (cmd_type) { |
|
|
|
// 参数个数*每个参数长度 = 实际参数长度 |
|
|
|
cmd_packet->valid_len = param->count * reg_info.data_len + 3; // cmd + id + check_sum = 3 |
|
|
|
cmd_packet->cmd = cmd; |
|
|
|
cmd_packet->param[0] = param->addr; |
|
|
|
// 判断参数数据长度 |
|
|
|
if (param_len == byte) { |
|
|
|
cmd.param[1] = (uint8_t)servo->data; |
|
|
|
} else if (param_len == word) { |
|
|
|
// 运行时间和位置校准需要特殊位的处理 |
|
|
|
if (kRegServoCalibration_map == servo->reg) { |
|
|
|
cmd.param[1] = servo->data & 0xff; // 低八位 |
|
|
|
cmd.param[2] = (servo->data >> 8) | (1 << BT_11); // 高八位 |
|
|
|
} else if (kRegServoRunTime_map == servo->reg) { |
|
|
|
cmd.param[1] = servo->data & 0xff; // 低八位 |
|
|
|
cmd.param[2] = (servo->data >> 8) | (1 << BT_10); // 高八位 |
|
|
|
} else { |
|
|
|
// 对于二进制来说 uint32_t和 int32_t是一样的 只是用最高位来区别正负 |
|
|
|
cmd.param[1] = ((uint32_t)servo->data) & 0xff; // 低八位 |
|
|
|
cmd.param[2] = ((uint32_t)servo->data) >> 8; // 高八位 |
|
|
|
printf("%02X\n", servo->data); |
|
|
|
} |
|
|
|
if (reg_info.data_len == byte) { |
|
|
|
memcpy(&cmd_packet->param[1], param->data, param->count); |
|
|
|
} else if (reg_info.data_len == word) { |
|
|
|
// 高低位交换，传入的寄存器地址不需要交换 |
|
|
|
high_low_exchange(param, &cmd_packet->param[1]); |
|
|
|
} |
|
|
|
} |
|
|
|
// else 同步写未完成 |
|
|
|
send_cmd_packet(&cmd); |
|
|
|
// else 同步和非同步无法合并为一个函数 |
|
|
|
check_sum = check_sum_calculate(cmd_packet->id, cmd_packet->valid_len, cmd_packet->cmd, param); |
|
|
|
memcpy(&cmd_packet->param[param->count * reg_info.data_len + 1], &check_sum, param->count); // 将checksum放入指令包末尾 |
|
|
|
|
|
|
|
tx_send_len = cmd_packet->valid_len + 4; // 4 = cmd_head[1] + cmd_head[2] + add + check_sum |
|
|
|
packet_process(tx_send_len); |
|
|
|
} |
|
|
|
|
|
|
|
/** |
|
|
|
@ -155,21 +167,18 @@ void write_reg(servo_obj_t* servo, uint8_t param_len) { |
|
|
|
* @param {servo_obj_t*} servo 舵机对象 |
|
|
|
* @return {*} |
|
|
|
*/ |
|
|
|
bool Write_oprete(servo_obj_t* servo) { |
|
|
|
uint8_t data_len; |
|
|
|
bool Write_oprete(uint8_t id, uint8_t cmd, param_t* param) { |
|
|
|
reg_distinguish(param); |
|
|
|
|
|
|
|
if (servo->cmd != SYC_W_DATA) |
|
|
|
if (cmd != SYC_W_DATA) |
|
|
|
cmd_type = non_syn; |
|
|
|
else |
|
|
|
cmd_type = syn; |
|
|
|
|
|
|
|
data_len = estimate_data_len(servo->reg); |
|
|
|
write_reg(id, cmd, param); |
|
|
|
return true; |
|
|
|
} |
|
|
|
|
|
|
|
if (data_len == error) { |
|
|
|
printf("The reg does not exist:%02X", servo->reg); |
|
|
|
return false; |
|
|
|
} |
|
|
|
void Ping_oprete(uint8_t id, uint8_t cmd){ |
|
|
|
|
|
|
|
write_reg(servo, data_len); |
|
|
|
return true; |
|
|
|
} |
sunlight
10 months ago