|
|
|
@ -1,5 +1,3 @@ |
|
|
|
|
|
|
|
|
|
|
|
#include "servo_package_process.h" |
|
|
|
|
|
|
|
#include <stdio.h> |
|
|
|
@ -7,169 +5,125 @@ |
|
|
|
|
|
|
|
#include "servo_driver.h" |
|
|
|
#include "servo_reg_map.h" |
|
|
|
static UART_HandleTypeDef* m_uart; |
|
|
|
|
|
|
|
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, |
|
|
|
}; |
|
|
|
|
|
|
|
void servo_uart_init(UART_HandleTypeDef* uart) { m_uart = uart; } |
|
|
|
#include "tick.h" |
|
|
|
|
|
|
|
|
|
|
|
static uint8_t rx_size; |
|
|
|
|
|
|
|
static bool Idle_Flag = false; |
|
|
|
|
|
|
|
void SET_IDEL_FLAG(void) { Idle_Flag = true; } |
|
|
|
void CLEAR_IDEL_FLAG(void) { Idle_Flag = false; } |
|
|
|
|
|
|
|
|
|
|
|
/** |
|
|
|
* @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_t* reg_info) { |
|
|
|
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)); |
|
|
|
uint8_t check_sum_calculate(uint8_t* basic, param_t* param) { |
|
|
|
// Check Sum = ~ (ID + Length + Instruction + Parameter1 + ... Parameter N) |
|
|
|
uint16_t i = 0; |
|
|
|
uint8_t basic_sum = 0; |
|
|
|
uint8_t sum = 0; |
|
|
|
for (i = 0; i < BASCI_LEN - 2; i++) { |
|
|
|
basic_sum += basic[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]); |
|
|
|
if (param != NULL) { |
|
|
|
sum = basic_sum + param->addr; |
|
|
|
for (i = 0; i < param->count; i++) { |
|
|
|
sum += (uint8_t)param->data[i]; |
|
|
|
} |
|
|
|
} else { |
|
|
|
sum = basic_sum; |
|
|
|
} |
|
|
|
|
|
|
|
return ~(sum); |
|
|
|
} |
|
|
|
|
|
|
|
/** |
|
|
|
* @description: 写寄存器(只实现了单个数据的写入 不支持多数据) |
|
|
|
* @param {servo_obj_t*} servo 舵机对象 |
|
|
|
* @param {uint8_t} param_len 数据写入寄存器中的长度 |
|
|
|
* @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; |
|
|
|
|
|
|
|
// 判断参数数据长度 |
|
|
|
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); |
|
|
|
} |
|
|
|
} |
|
|
|
void high_low_exchange(param_t* param, uint8_t* packet, reg_info_t* reg_info) { |
|
|
|
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)); |
|
|
|
} |
|
|
|
// else 同步写未完成 |
|
|
|
send_cmd_packet(&cmd); |
|
|
|
} |
|
|
|
|
|
|
|
/** |
|
|
|
* @description: 写操作 |
|
|
|
* @param {servo_obj_t*} servo 舵机对象 |
|
|
|
* @return {*} |
|
|
|
*/ |
|
|
|
bool Write_oprete(servo_obj_t* servo) { |
|
|
|
uint8_t data_len; |
|
|
|
|
|
|
|
if (servo->cmd != SYC_W_DATA) |
|
|
|
cmd_type = non_syn; |
|
|
|
else |
|
|
|
cmd_type = syn; |
|
|
|
|
|
|
|
data_len = estimate_data_len(servo->reg); |
|
|
|
void packet_process(UART_HandleTypeDef* uart, uint8_t* tx, uint8_t* rx, uint8_t len) { |
|
|
|
cmd_receive_t* receive = (cmd_receive_t*)rx; |
|
|
|
uint32_t lasttime = HAL_GetTick(); |
|
|
|
|
|
|
|
HAL_UARTEx_ReceiveToIdle_DMA(uart, rx, sizeof(rx)); |
|
|
|
HAL_UART_Transmit(uart, tx, len, 100); |
|
|
|
// HAL_UART_Receive_DMA(); |
|
|
|
|
|
|
|
if (data_len == error) { |
|
|
|
printf("The reg does not exist:%02X", servo->reg); |
|
|
|
return false; |
|
|
|
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(); |
|
|
|
} |
|
|
|
|
|
|
|
write_reg(servo, data_len); |
|
|
|
return true; |
|
|
|
printf("tx:%s\n", tx); |
|
|
|
printf("tx_send_len: %02X\n", len); |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// void get_rx_size(uint16_t Size) { rx_size = Size - __HAL_DMA_GET_COUNTER(m_uart->hdmarx); } |
sunlight