200 lines
6.9 KiB
C
200 lines
6.9 KiB
C
/**
|
||
*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(ledGreen_GPIO_Port, ledGreen_Pin, GPIO_PIN_SET); //蓝色灯亮 // indicate CRC incorrect
|
||
HAL_GPIO_WritePin(ledBlue_GPIO_Port, ledBlue_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(ledBlue_GPIO_Port, ledBlue_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];
|
||
}
|
||
|