109 lines
3.4 KiB
C
109 lines
3.4 KiB
C
/**
|
|
*go控制程序
|
|
*/
|
|
#include "GO_M8010_6_Driver.h"
|
|
#include "usart.h"
|
|
#include "bsp_usart.h"
|
|
#include <cmsis_os2.h>
|
|
#include "crc16.h"
|
|
#include <string.h>
|
|
|
|
#define SATURATE(_IN, _MIN, _MAX) \
|
|
{ \
|
|
if ((_IN) <= (_MIN)) \
|
|
(_IN) = (_MIN); \
|
|
else if ((_IN) >= (_MAX)) \
|
|
(_IN) = (_MAX); \
|
|
}
|
|
GO_MotorCmd_t cmd_gogogo ;
|
|
RIS_ControlData_t motor_send_data;
|
|
|
|
|
|
GO_MotorData_t data = {0}; // 确保 data 定义在此处
|
|
uint8_t rx_buffer[sizeof(data.motor_recv_data)] = {0}; // 确保 rx_buffer 定义在此处
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/// @brief 将发送给电机的浮点参数转换为定点类型参数
|
|
/// @param motor_s 要转换的电机指令结构体
|
|
void modify_data(GO_MotorCmd_t *motor_s)
|
|
{
|
|
motor_send_data.head[0] = 0xFE;
|
|
motor_send_data.head[1] = 0xEE;
|
|
|
|
SATURATE(motor_s->id, 0, 15);
|
|
SATURATE(motor_s->mode, 0, 7);
|
|
SATURATE(motor_s->K_P, 0.0f, 25.599f);
|
|
SATURATE(motor_s->K_W, 0.0f, 25.599f);
|
|
SATURATE(motor_s->T, -127.99f, 127.99f);
|
|
SATURATE(motor_s->W, -804.00f, 804.00f);
|
|
SATURATE(motor_s->Pos, -411774.0f, 411774.0f);
|
|
|
|
motor_send_data.mode.id = motor_s->id;
|
|
motor_send_data.mode.status = motor_s->mode;
|
|
motor_send_data.comd.k_pos = motor_s->K_P / 25.6f * 32768.0f;
|
|
motor_send_data.comd.k_spd = motor_s->K_W / 25.6f * 32768.0f;
|
|
motor_send_data.comd.pos_des = motor_s->Pos / 6.28318f * 32768.0f;
|
|
motor_send_data.comd.spd_des = motor_s->W / 6.28318f * 256.0f;
|
|
motor_send_data.comd.tor_des = motor_s->T * 256.0f;
|
|
motor_send_data.CRC16 = CRC16_Calc( (uint8_t *)&motor_send_data, sizeof(RIS_ControlData_t) - sizeof(motor_send_data.CRC16),0);
|
|
}
|
|
|
|
/// @brief 将接收到的定点类型原始数据转换为浮点参数类型
|
|
/// @param motor_r 要转换的电机反馈结构体
|
|
void extract_data(GO_MotorData_t *motor_r)
|
|
{
|
|
if (motor_r->motor_recv_data.head[0] != 0xFD || motor_r->motor_recv_data.head[1] != 0xEE)
|
|
{
|
|
motor_r->correct = 0;
|
|
return;
|
|
}
|
|
motor_r->calc_crc = CRC16_Calc((uint8_t *)&motor_r->motor_recv_data, sizeof(RIS_MotorData_t) - sizeof(motor_r->motor_recv_data.CRC16),0);
|
|
if (motor_r->motor_recv_data.CRC16 != motor_r->calc_crc)
|
|
{
|
|
memset(&motor_r->motor_recv_data, 0, sizeof(RIS_MotorData_t));
|
|
motor_r->correct = 0;
|
|
motor_r->bad_msg++;
|
|
return;
|
|
}
|
|
else
|
|
{
|
|
motor_r->motor_id = motor_r->motor_recv_data.mode.id;
|
|
motor_r->mode = motor_r->motor_recv_data.mode.status;
|
|
motor_r->Temp = motor_r->motor_recv_data.fbk.temp;
|
|
motor_r->MError = motor_r->motor_recv_data.fbk.MError;
|
|
motor_r->W = ((float)motor_r->motor_recv_data.fbk.speed / 256.0f) * 6.28318f;
|
|
motor_r->T = ((float)motor_r->motor_recv_data.fbk.torque) / 256.0f;
|
|
motor_r->Pos = 6.28318f * ((float)motor_r->motor_recv_data.fbk.pos) / 32768.0f;
|
|
motor_r->footForce = motor_r->motor_recv_data.fbk.force;
|
|
motor_r->correct = 1;
|
|
return;
|
|
}
|
|
}
|
|
void GO_M8010_send_data(GO_MotorCmd_t *cmd)
|
|
{
|
|
modify_data(cmd);
|
|
|
|
HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_RS485), (uint8_t *)(&motor_send_data), sizeof(motor_send_data));
|
|
osDelay (1);
|
|
HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_RS485), rx_buffer, sizeof(rx_buffer));
|
|
|
|
}
|
|
|
|
void USART6_RxCompleteCallback(void )
|
|
{
|
|
|
|
UART_HandleTypeDef *huart6 = BSP_UART_GetHandle(BSP_UART_RS485);
|
|
uint16_t crc = CRC16_Calc(rx_buffer,sizeof(rx_buffer)-2,0x0000);
|
|
|
|
memcpy(&data.motor_recv_data, rx_buffer, sizeof(data.motor_recv_data)); // Copy received data
|
|
extract_data(&data);
|
|
}
|
|
const GO_MotorData_t *get_GO_measure_point(void)
|
|
{
|
|
return &data;
|
|
}
|