shoot/User/device/GO_M8010_6_Driver.c

200 lines
6.9 KiB
C
Raw Normal View History

2025-03-12 23:04:18 +08:00
/**
*go控制程序
*/
#include "GO_M8010_6_Driver.h"
#include "usart.h"
GO_Motorfield GO_motor_info[GO_NUM];//存放电机数据
const float gravit_const =9.81;//计算前馈力矩有关
//数据处理函数
static void GO_M8010_recv_data(uint8_t* Temp_buffer);
// void GO_M8010_recv_data(uint8_t* Temp_buffer);
/**
*@brief
*/
void GO_M8010_init (){
for (uint8_t id= 0; id <GO_NUM ;id++)
{
GO_motor_info[id].id = id;
GO_motor_info[id].mode = 1;
GO_motor_info[id].correct = 1;
GO_motor_info[id].MError = 0;
GO_motor_info[id].Temp = 0;
GO_motor_info[id].tar_pos = 0;
GO_motor_info[id].tar_w = 0;
GO_motor_info[id].T = 0;
GO_motor_info[id].W = 0;
GO_motor_info[id].Pos = 0;
GO_motor_info[id].footForce = 0;
GO_motor_info[id].buffer[0] = 0xFE;
GO_motor_info[id].buffer[1] = 0xEE;
}
// HAL_UART_RegisterCallback(&huart1, HAL_UART_TX_COMPLETE_CB_ID, uartTxCB);
HAL_UART_RegisterCallback(&huart6, HAL_UART_TX_COMPLETE_CB_ID, uartTxCB);
}
//暂存接收数据
static uint8_t Temp_buffer[16];
//dma传输完成后进入此回调函数
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
// CRC checking. if CRC checking fail the led will indicate and return
uint16_t crc = do_crc_table(Temp_buffer,sizeof(Temp_buffer)-2);
if ((Temp_buffer[14] != (crc&0xFF)) || (Temp_buffer[15] != ((crc>>8) & 0xFF)))
{
HAL_GPIO_WritePin(LED_B_GPIO_Port, LED_B_Pin, GPIO_PIN_SET); //蓝色灯亮 // indicate CRC incorrect
HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_G_Pin, GPIO_PIN_RESET); //红色灯灭
return;
}
// CRC pass and start converting data to the motor
HAL_GPIO_WritePin(GPIOH, GPIO_PIN_11, GPIO_PIN_SET); // indicate CRC correct
HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_G_Pin, GPIO_PIN_RESET); //红色灯灭
GO_M8010_recv_data(Temp_buffer);
}
/**
*@brief
*/
void uartTxCB(UART_HandleTypeDef *huart)
{
//发送完成后开启接受dma并将传输完成中断回调函数指向串口接收完成中断回调函数
HAL_UART_Receive_DMA(huart,Temp_buffer,sizeof(Temp_buffer));
}
/**
*@brief go发送控制命令
*@input[in] huart
*@input[in] id id
*@input[in] rev github为回答issue
*@input[in] T N·m
*@input[in] Pos rad
*@input[in] W rad/s
*@input[in] K_P kp
*@input[in] K_W kp
*@note t = T + - *K_P + - *K_W
*/
uint8_t xx;
void GO_M8010_send_data(UART_HandleTypeDef *huart, int id,int rev,float T,float Pos,
float W,float K_P,float K_W)
{
// a pointer to target motor
GO_Motorfield* motor;
motor = GO_motor_info+id;
//这个rev是干啥的
// send preious cmd
if (rev==1){
HAL_UART_Transmit_IT(huart,motor->buffer,sizeof(motor->buffer));
return;
}
// assign motor target goal to the buffer
motor->motor_send_data.head[0]=0xFE;
motor->motor_send_data.head[1]=0xEE;
motor->motor_send_data.mode.id = motor->id & 0xF;
motor->motor_send_data.mode.status = motor->mode & 0x7;
motor->motor_send_data.mode.none = 0x0;
motor->motor_send_data.comd.tor_des = T*256;
motor->motor_send_data.comd.spd_des = (W*256*6.33f)/(6.28319f);
motor->motor_send_data.comd.pos_des = (Pos*32768*6.33f)/(6.28319f);
motor->motor_send_data.comd.k_pos = K_P*1280;
motor->motor_send_data.comd.k_spd = K_W*1280;
uint8_t* mode = (uint8_t *)&(motor->motor_send_data.mode);
motor->buffer[2] = *mode;
motor->buffer[3] = (motor->motor_send_data.comd.tor_des) & 0xFF;
motor->buffer[4] = (motor->motor_send_data.comd.tor_des>>8) & 0xFF;
motor->buffer[5] = (motor->motor_send_data.comd.spd_des) & 0xFF;
motor->buffer[6] = (motor->motor_send_data.comd.spd_des>>8) & 0xFF;
motor->buffer[7] = (motor->motor_send_data.comd.pos_des) & 0xFF;
motor->buffer[8] = (motor->motor_send_data.comd.pos_des>>8) & 0xFF;
motor->buffer[9] = (motor->motor_send_data.comd.pos_des>>16) & 0xFF;
motor->buffer[10] = (motor->motor_send_data.comd.pos_des>>24) & 0xFF;
motor->buffer[11] = (motor->motor_send_data.comd.k_pos) & 0xFF;
motor->buffer[12] = (motor->motor_send_data.comd.k_pos>>8) & 0xFF;
motor->buffer[13] = (motor->motor_send_data.comd.k_spd) & 0xFF;
motor->buffer[14] = (motor->motor_send_data.comd.k_spd>>8) & 0xFF;
//crc calulation
uint16_t crc = do_crc_table(motor->buffer, sizeof(motor->buffer)-2);
motor->buffer[15] = (crc) & 0xFF;
motor->buffer[16] = (crc>>8) & 0xFF;
// interrupt buffer sending
xx = HAL_UART_Transmit_IT(huart,motor->buffer,sizeof(motor->buffer));
HAL_Delay(100);
HAL_UART_Receive_DMA(huart, Temp_buffer, 16);//有无都可以
}
//数据处理函数
void GO_M8010_recv_data(uint8_t* Temp_buffer)
{
// check for ID
int8_t ID=-1;
GO_Motorfield* motor;
ID = Temp_buffer[2] & 0xF;
// a pointer to target pointer
motor = GO_motor_info + ID;
memcpy(motor->Rec_buffer,Temp_buffer,16);
// assign buffer to the target motor
motor->mode = Temp_buffer[2]>>4 & 0xF;
motor->correct = 1;
motor->MError = 0;
int16_t T = Temp_buffer[4]<<8 | Temp_buffer[3];
motor->T = T;
motor->T /= 256;
int16_t W = Temp_buffer[6]<<8 | Temp_buffer[5];
motor->W = W;
motor->W = (motor->W * 6.28319f )/(256*6.33f);
int32_t Pos = Temp_buffer[10]<<24 | Temp_buffer[9]<<16 | Temp_buffer[8]<<8 | Temp_buffer[7];
motor->Pos = Pos;
motor->Pos = (motor->Pos * 6.28319f )/(32768*6.33f);
int8_t Temp = Temp_buffer[11] & 0xFF;
motor->Temp = Temp;
motor->MError = Temp_buffer[12] & 0x7;
}
//力位混合控制
// for differnt robot should have different define. Now just for testing first
/**
*@brief
*@input[in] huart
*@input[in] id id
*@input[in] bias
*@input[in] length
*@input[in] mass
*@input[in] tar_pos
*@input[in] tar_w
*@input[in] K_P kp
*@input[in] K_W kp
*/
void basic_ForceControl (UART_HandleTypeDef *huart, int id, float bias, float length, float mass,
float tar_pos, float tar_w, float K_P,float K_W)
{
float forw_T = mass* gravit_const* length* (cos(GO_motor_info[id].Pos));//计算前馈力矩
forw_T += bias;
GO_M8010_send_data(huart,id,0,forw_T,tar_pos,tar_w,K_P,K_W);
}
/**
*@brief go发送控制命令
*@input[in] id id
*@revtal
*/
GO_Motorfield* getGoPoint(uint8_t id)
{
return &GO_motor_info[id];
}