LYL/R2_UP
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

重改go

Browse Source
This commit is contained in:
ZHAISHUI04 2025-04-20 14:06:43 +08:00
parent 0ef286e5f2
commit b12bcd25d6
17 changed files with 349 additions and 362 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
AUTO_CHASSIS.ioc
View File

@ -28,7 +28,8 @@ Dma.Request2=USART3_RX
Dma.Request3=USART6_RX
Dma.Request4=USART1_TX
Dma.Request5=USART1_RX
Dma.RequestsNb=6
Dma.Request6=USART6_TX
Dma.RequestsNb=7
Dma.SPI1_RX.0.Direction=DMA_PERIPH_TO_MEMORY
Dma.SPI1_RX.0.FIFOMode=DMA_FIFOMODE_DISABLE
Dma.SPI1_RX.0.Instance=DMA2_Stream2
@ -89,6 +90,16 @@ Dma.USART6_RX.3.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
Dma.USART6_RX.3.PeriphInc=DMA_PINC_DISABLE
Dma.USART6_RX.3.Priority=DMA_PRIORITY_VERY_HIGH
Dma.USART6_RX.3.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode
Dma.USART6_TX.6.Direction=DMA_MEMORY_TO_PERIPH
Dma.USART6_TX.6.FIFOMode=DMA_FIFOMODE_DISABLE
Dma.USART6_TX.6.Instance=DMA2_Stream6
Dma.USART6_TX.6.MemDataAlignment=DMA_MDATAALIGN_BYTE
Dma.USART6_TX.6.MemInc=DMA_MINC_ENABLE
Dma.USART6_TX.6.Mode=DMA_NORMAL
Dma.USART6_TX.6.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
Dma.USART6_TX.6.PeriphInc=DMA_PINC_DISABLE
Dma.USART6_TX.6.Priority=DMA_PRIORITY_LOW
Dma.USART6_TX.6.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode
FREERTOS.INCLUDE_pcTaskGetTaskName=1
FREERTOS.INCLUDE_uxTaskGetStackHighWaterMark2=1
FREERTOS.INCLUDE_vTaskCleanUpResources=1
@ -178,6 +189,7 @@ NVIC.DMA2_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream2_IRQn=true\:5\:0\:true\:false\:true\:false\:false\:true\:true
NVIC.DMA2_Stream3_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream5_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream6_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream7_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
NVIC.EXTI0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true

1
Core/Inc/stm32f4xx_it.h
View File

@ -69,6 +69,7 @@ void CAN2_RX0_IRQHandler(void);
void CAN2_RX1_IRQHandler(void);
void OTG_FS_IRQHandler(void);
void DMA2_Stream5_IRQHandler(void);
void DMA2_Stream6_IRQHandler(void);
void DMA2_Stream7_IRQHandler(void);
void USART6_IRQHandler(void);
/* USER CODE BEGIN EFP */

3
Core/Src/dma.c
View File

@ -59,6 +59,9 @@ void MX_DMA_Init(void)
/* DMA2_Stream5_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream5_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream5_IRQn);
/* DMA2_Stream6_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream6_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream6_IRQn);
/* DMA2_Stream7_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream7_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream7_IRQn);

15
Core/Src/stm32f4xx_it.c
View File

@ -69,6 +69,7 @@ extern DMA_HandleTypeDef hdma_usart1_tx;
extern DMA_HandleTypeDef hdma_usart1_rx;
extern DMA_HandleTypeDef hdma_usart3_rx;
extern DMA_HandleTypeDef hdma_usart6_rx;
extern DMA_HandleTypeDef hdma_usart6_tx;
extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart6;
/* USER CODE BEGIN EV */
@ -418,6 +419,20 @@ void DMA2_Stream5_IRQHandler(void)
/* USER CODE END DMA2_Stream5_IRQn 1 */
}
/**
* @brief This function handles DMA2 stream6 global interrupt.
*/
void DMA2_Stream6_IRQHandler(void)
{
/* USER CODE BEGIN DMA2_Stream6_IRQn 0 */
/* USER CODE END DMA2_Stream6_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart6_tx);
/* USER CODE BEGIN DMA2_Stream6_IRQn 1 */
/* USER CODE END DMA2_Stream6_IRQn 1 */
}
/**
* @brief This function handles DMA2 stream7 global interrupt.
*/

20
Core/Src/usart.c
View File

@ -31,6 +31,7 @@ DMA_HandleTypeDef hdma_usart1_tx;
DMA_HandleTypeDef hdma_usart1_rx;
DMA_HandleTypeDef hdma_usart3_rx;
DMA_HandleTypeDef hdma_usart6_rx;
DMA_HandleTypeDef hdma_usart6_tx;
/* USART1 init function */
@ -278,6 +279,24 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
__HAL_LINKDMA(uartHandle,hdmarx,hdma_usart6_rx);
/* USART6_TX Init */
hdma_usart6_tx.Instance = DMA2_Stream6;
hdma_usart6_tx.Init.Channel = DMA_CHANNEL_5;
hdma_usart6_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
hdma_usart6_tx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_usart6_tx.Init.MemInc = DMA_MINC_ENABLE;
hdma_usart6_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_usart6_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_usart6_tx.Init.Mode = DMA_NORMAL;
hdma_usart6_tx.Init.Priority = DMA_PRIORITY_LOW;
hdma_usart6_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
if (HAL_DMA_Init(&hdma_usart6_tx) != HAL_OK)
{
Error_Handler();
}
__HAL_LINKDMA(uartHandle,hdmatx,hdma_usart6_tx);
/* USART6 interrupt Init */
HAL_NVIC_SetPriority(USART6_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(USART6_IRQn);
@ -352,6 +371,7 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
/* USART6 DMA DeInit */
HAL_DMA_DeInit(uartHandle->hdmarx);
HAL_DMA_DeInit(uartHandle->hdmatx);
/* USART6 interrupt Deinit */
HAL_NVIC_DisableIRQ(USART6_IRQn);

15
MDK-ARM/AUTO_CHASSIS.uvoptx
View File

@ -158,32 +158,27 @@
<Ww>
<count>0</count>
<WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText>
<ItemText>GO_motor_info</ItemText>
</Ww>
<Ww>
<count>1</count>
<WinNumber>1</WinNumber>
<ItemText>GO_motor_info</ItemText>
<ItemText>aaa</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>dr16,0x0A</ItemText>
<ItemText>UP,0x0A</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>cmd,0x0A</ItemText>
<ItemText>aaa</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>rc_ctrl,0x0A</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>aaa</ItemText>
<ItemText>motor_send_data,0x0A</ItemText>
</Ww>
</WatchWindow1>
<WatchWindow2>

BIN
MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf
View File

Binary file not shown.

34
User/Module/config.c
View File

@ -66,29 +66,19 @@ static const ConfigParam_t param_chassis ={
.out_limit = 3000.0f
},
.go_param[0]={
.rev = 0,
.T=0.79,
.W=0.1,
.K_P=0.2,
.K_W=0.1,
},
.go_param[1]={
.rev = 0,
.T=0,
.W=0,
.K_P=1.0f,
.K_W=0.1,
},
.go1_position_param={
.p =0.1f,
.i =0.1f,
.d =0.0f,
.i_limit = 2.0f,
.out_limit = 10.0f
.go_cmd={
.id =0,
.mode = 1,
.K_P = 1.0f,
.K_W = 0.05,
.Pos = 0,
.W = 0,
.T = 0,
},
/*上层其他参数*/
/*运球*/
.DribbleConfig_Config = {
@ -103,8 +93,8 @@ static const ConfigParam_t param_chassis ={
.PitchConfig_Config = {
.m2006_init_angle =-150,
.m2006_trigger_angle =0,
.go1_init_position = -500,
.go1_release_threshold =-1900,
.go1_init_position = 0,
.go1_release_threshold =0,
.m2006_Screw_init=0
},

102
User/Module/up.c
View File

@ -19,8 +19,7 @@
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
{
u->param = param; /*初始化参数 */
/*go电机初始化*/
GO_M8010_init();
/*pid初始化*/
PID_init (&u->pid.VESC_5065_M1 ,PID_POSITION ,&(u->param ->VESC_5065_M1_param ));
PID_init (&u->pid.VESC_5065_M2 ,PID_POSITION ,&(u->param ->VESC_5065_M2_param ));
@ -34,11 +33,7 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
PID_init (&u->pid .GM6020_speed,PID_POSITION ,&(u->param ->UP_GM6020_speed_param ));
PID_init (&u->pid .GM6020_angle,PID_POSITION ,&(u->param ->UP_GM6020_angle_param ));
PID_init (&u->pid .GO1_position ,PID_POSITION,&(u->param->go1_position_param ));
// for(int i=0;i<2;i++){ //go初始位置设置为0
// GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), i,u->param->go_param[i] .rev ,u->param->go_param[i] .T ,u->param->go_param[i] .W ,0,u->param->go_param [i].K_P ,u->param->go_param[i] .K_W );
// }
u->go_cmd =u->param ->go_cmd ;
// 初始化上层状态机
if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球
@ -54,14 +49,14 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
u->PitchContext .is_initialized = 1;
}
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
}
/*can，上层状态更新*/
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
for(int i=0;i<2;i++){ //go初始位置设置为0
u->motorfeedback .GO_motor_info [i] = getGoPoint(i);
}
u->motorfeedback .go_data = get_GO_measure_point() ;
u->motorfeedback .M2006 .motor =M2006;
u->motorfeedback .M3508 .motor =M3508;
@ -195,18 +190,39 @@ int8_t GM6020_control(UP_t *u,fp32 angle)
}
/*go电机控制*/
int8_t GO_SendData(UP_t *u,int id,float pos)
int8_t GO_SendData(UP_t *u,float pos,float limit)
{
fp32 deal_pos;
u->go_cmd .Pos =pos;
// 读取参数
float tff = u->go_cmd.T ; // 前馈力矩
float kp = u->go_cmd .K_P; // 刚度
float kd = u->go_cmd.K_W; // 阻尼（原代码中kdd/kid混淆，需统一为K_W）
float q_desired = u->go_cmd.Pos; // 期望位置（rad）
float q_current = u->motorfeedback.go_data ->Pos ; // 当前位置（rad）
float dq_desired = u->go_cmd.W; // 期望速度（rad/s）
float dq_current = u->motorfeedback.go_data ->W ; // 当前速度（rad/s）
float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current);
if (tau > limit) {
tff = limit - kp * (q_desired - q_current) - kd * (dq_desired - dq_current) - (tau - limit) / 2.0;
tau = limit;
} else if (tau < -limit) {
tff = limit - kp * (q_desired - q_current) - kd * (dq_desired - dq_current) + (tau - limit) / 2.0;
tau = -limit;
}
u->go_cmd .T =tff;
GO_M8010_send_data(&u->go_cmd );
u->go_cmd .T =0;
// deal_pos = PID_calc (&u->pid .GO1_position ,u->motorfeedback .GO_motor_info [0]->Pos ,pos);
GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), id,u->param->go_param[id] .rev ,u->param->go_param[id].T ,u->param->go_param[id] .W ,pos,u->param->go_param [id].K_P ,u->param->go_param[id] .K_W );
return 0;
}
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
{
//电机控制 ，传进can
@ -216,7 +232,7 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
GO_SendData(u,0 ,u->motor_target .go_shoot );
GO_SendData(u,u->motor_target .go_shoot,0.08 );
for(int i=0;i<4;i++){
@ -338,11 +354,11 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
case PITCH_START:
u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold;
if(u->motorfeedback .GO_motor_info[0]->Pos < -32.40){ //检测go位置到达最上面，这里的检测条件可以更改
u->motor_target .M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0
// if(u->motorfeedback .GO_motor_info[0]->Pos < -32.40){ //检测go位置到达最上面，这里的检测条件可以更改
// u->motor_target .M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0
u->PitchContext .PitchState=PITCH_PULL_TRIGGER;
}//更改标志位
//}//更改标志位
break ;
case PITCH_PULL_TRIGGER:
@ -413,10 +429,10 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
RELAY2_TOGGLE (1);
if(((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.1)&&(u->motorfeedback.M3508 .total_angle >1190)){
u->DribbleContext .DribbleState = STATE_RELEASE_BALL; //当go2到标准位置，标志位改变
}
// if(((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.1)&&(u->motorfeedback.M3508 .total_angle >1190)){
//
// u->DribbleContext .DribbleState = STATE_RELEASE_BALL; //当go2到标准位置，标志位改变
// }
}
break;
@ -428,20 +444,20 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
if((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.3){
u->motor_target.go_spin =-40; //恢复go2位置
u->DribbleContext .DribbleState = STATE_CATCH_PREP; //当go2到标准位置，标志位改变
// if((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.3){
// u->motor_target.go_spin =-40; //恢复go2位置
// u->DribbleContext .DribbleState = STATE_CATCH_PREP; //当go2到标准位置，标志位改变
}
// }
break;
case STATE_CATCH_PREP:
if((u->motorfeedback.GO_motor_info[1]->Pos )> -0.4){
u->motor_target.M3508_angle =0 ; //当go2到初始位置，3508降
RELAY2_TOGGLE (0);//接球
}
// if((u->motorfeedback.GO_motor_info[1]->Pos )> -0.4){
// u->motor_target.M3508_angle =0 ; //当go2到初始位置，3508降
//
// RELAY2_TOGGLE (0);//接球
// }
if(u->motorfeedback .M3508 .total_angle <51.0f){
RELAY1_TOGGLE (0);//夹球，0夹1开
@ -477,15 +493,15 @@ void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out)
u->motor_target .go_spin =u->DribbleContext .DribbleConfig .go2_release_threshold ;
}
if(u->motorfeedback .GO_motor_info [1]->Pos < -4.9)
{
RELAY1_TOGGLE (1);//夹球，0夹1开
if(u->motorfeedback .GO_motor_info [1]->Pos > -4.8)
{
u->DribbleContext .DribbleState = STATE_CATCH_DONE; //当go2到标准位置，标志位改变
}
}
// if(u->motorfeedback .GO_motor_info [1]->Pos < -4.9)
// {
// RELAY1_TOGGLE (1);//夹球，0夹1开
//
// if(u->motorfeedback .GO_motor_info [1]->Pos > -4.8)
// {
// u->DribbleContext .DribbleState = STATE_CATCH_DONE; //当go2到标准位置，标志位改变
// }
// }
break ;

13
User/Module/up.h
View File

@ -154,11 +154,10 @@ typedef struct
pid_param_t M3508_speed_param;
pid_param_t M3508_angle_param;
GO_param_t go_param[2];
pid_param_t go1_position_param;
DribbleConfig_t DribbleConfig_Config;
PitchConfig_t PitchConfig_Config;
GO_MotorCmd_t go_cmd;
}UP_Param_t;
typedef struct
@ -205,7 +204,7 @@ typedef struct{
motor_angle_data M2006;
motor_angle_data M3508;
GO_Motorfield *GO_motor_info[GO_NUM];//存放go电机数据
const GO_MotorData_t *go_data;//存放go电机数据
fp32 M3508_angle[4];
fp32 M3508_rpm [4];
@ -245,9 +244,9 @@ typedef struct{
pid_type_def M3508_angle;
pid_type_def M3508_speed;
pid_type_def GO1_position;
}pid;
GO_MotorCmd_t go_cmd;
@ -276,7 +275,7 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq);
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) ;
int8_t GM6020_control(UP_t *u,fp32 angle);
int8_t VESC_M5065_Control(UP_t *u,fp32 speed);
int8_t GO_SendData(UP_t *u,int id,float pos);
int8_t GO_SendData(UP_t *u,float pos,float limit);
int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor);
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c);
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out);

6
User/bsp/crc16.c
View File

@ -39,6 +39,12 @@ uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc) {
while (len--) crc = CRC16_Byte(crc, *buf++);
return crc;
}
//uint16_t crc_ccitt(uint16_t crc, uint8_t const *buffer, size_t len)
//{
// while (len--)
// crc = crc_ccitt_byte(crc, *buffer++);
// return crc;
//}
bool CRC16_Verify(const uint8_t *buf, size_t len) {
if (len < 2) return false;

267
User/device/GO_M8010_6_Driver.c
View File

@ -5,200 +5,109 @@
#include "usart.h"
#include "bsp_usart.h"
#include <cmsis_os2.h>
#include "crc16.h"
#include <string.h>
GO_Motorfield GO_motor_info[GO_NUM];//存放电机数据
static const float gravit_const =9.81;//计算前馈力矩有关
#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;
//数据处理函数
static void GO_M8010_recv_data(uint8_t* Temp_buffer);
/**
*@brief 电机初始化
*/
void GO_M8010_init (void){
for (uint8_t id= 0; id <GO_NUM ;id++)
{
GO_motor_info[id].id = id;
GO_motor_info[id].mode = 1; //foc闭环
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(BSP_UART_GetHandle(BSP_UART_RS485), HAL_UART_TX_COMPLETE_CB_ID, uartTxCB);
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
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);
//暂存接收数据
uint8_t Temp_buffer[16];
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(Temp_buffer,sizeof(Temp_buffer)-2,0x0000);
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); //蓝色灯亮
// HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET); //红色灯灭
return;
}
// HAL_GPIO_WritePin(GPIOH, GPIO_PIN_11, GPIO_PIN_SET); // indicate CRC correct
// HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET); //红色灯灭
GO_M8010_recv_data(Temp_buffer);
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);
}
/**
*@brief 用户自定义串口发送完成中断回调函数
*/
void uartTxCB(UART_HandleTypeDef *huart)
const GO_MotorData_t *get_GO_measure_point(void)
{
//发送完成后开启接受dma，并将传输完成中断回调函数指向串口接收完成中断回调函数
HAL_UART_Receive_DMA(huart,Temp_buffer,sizeof(Temp_buffer));
return &data;
}
/**
*@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
*/
void GO_M8010_send_data(UART_HandleTypeDef *huart, int id,int rev,float T,float Pos,
float W,float K_P,float K_W)
void gogogo(void )
{
GO_Motorfield* motor;
// a pointer to target 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 = CRC16_Calc(motor->buffer, sizeof(motor->buffer)-2,0x0000);
motor->buffer[15] = (crc) & 0xFF;
motor->buffer[16] = (crc>>8) & 0xFF;
// interrupt buffer sending
HAL_UART_Transmit_IT(huart,motor->buffer,sizeof(motor->buffer));
osDelay(1);
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[7] << 0) |
(Temp_buffer[8] << 8) |
(Temp_buffer[9] << 16) |
(Temp_buffer[10] << 24);
if(Pos > 0x7FFFFFFF) Pos -= 0xFFFFFFFF; // 溢出修正
motor->Pos = ((float)Pos / 32768.0f) * (6.28319f / 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];
}

162
User/device/GO_M8010_6_Driver.h
View File

@ -1,94 +1,116 @@
#ifndef __GO_M8010_6_H
#define __GO_M8010_6_H
#ifdef __cplusplus
extern "C"{
#endif
#include "main.h"
#include "crc16.h"
#include "usart.h"
#include "string.h"
#include <math.h>
#include "user_math.h"
#define GO_NUM 2
/**
* @brief
#include "struct_typedef.h"
#pragma pack(1)
/**
* @brief 电机模式控制信息
*
*/
typedef struct
{
uint8_t id :4;
uint8_t status :3;
uint8_t none :1;
} RIS_Mode_t ;
typedef struct
{
int16_t tor_des;
int16_t spd_des;
int32_t pos_des;
uint16_t k_pos;
uint16_t k_spd;
} RIS_Comd_t;
uint8_t id : 4; // 电机ID: 0,1...,13,14 15表示向所有电机广播数据(此时无返回)
uint8_t status : 3; // 工作模式: 0.锁定 1.FOC闭环 2.编码器校准 3.保留
uint8_t reserve : 1; // 保留位
} RIS_Mode_t; // 控制模式 1Byte
/**
*
* @brief 电机状态控制信息
*
*/
typedef struct
{
uint8_t head[2];
RIS_Mode_t mode;
RIS_Comd_t comd;
uint16_t CRC16;
int16_t tor_des; // 期望关节输出扭矩 unit: N.m (q8)
int16_t spd_des; // 期望关节输出速度 unit: rad/s (q8)
int32_t pos_des; // 期望关节输出位置 unit: rad (q15)
int16_t k_pos; // 期望关节刚度系数 unit: -1.0-1.0 (q15)
int16_t k_spd; // 期望关节阻尼系数 unit: -1.0-1.0 (q15)
} ControlData_t;
} RIS_Comd_t; // 控制参数 12Byte
/**
* @brief 电机状态反馈信息
*
*/
typedef struct
{
int16_t torque; // 实际关节输出扭矩 unit: N.m (q8)
int16_t speed; // 实际关节输出速度 unit: rad/s (q8)
int32_t pos; // 实际关节输出位置 unit: rad (q15)
int8_t temp; // 电机温度: -128~127°C
uint8_t MError : 3; // 电机错误标识: 0.正常 1.过热 2.过流 3.过压 4.编码器故障 5-7.保留
uint16_t force : 12; // 足端气压传感器数据 12bit (0-4095)
uint8_t none : 1; // 保留位
} RIS_Fbk_t; // 状态数据 11Byte
/**
* @brief 控制数据包格式
*
*/
typedef struct
{
uint8_t head[2]; // 包头 2Byte
RIS_Mode_t mode; // 电机控制模式 1Byte
RIS_Comd_t comd; // 电机期望数据 12Byte
uint16_t CRC16; // CRC 2Byte
} RIS_ControlData_t; // 主机控制命令 17Byte
/**
* @brief 电机反馈数据包格式
*
*/
typedef struct
{
uint8_t head[2]; // 包头 2Byte
RIS_Mode_t mode; // 电机控制模式 1Byte
RIS_Fbk_t fbk; // 电机反馈数据 11Byte
uint16_t CRC16; // CRC 2Byte
} RIS_MotorData_t; // 电机返回数据 16Byte
#pragma pack()
/// @brief 电机指令结构体
typedef struct
{
unsigned short id; // 电机ID，15代表广播数据包
unsigned short mode; // 0:空闲 1:FOC控制 2:电机标定
float T; // 期望关节的输出力矩(电机本身的力矩)(Nm)
float W; // 期望关节速度(电机本身的速度)(rad/s)
float Pos; // 期望关节位置(rad)
float K_P; // 关节刚度系数(0-25.599)
float K_W; // 关节速度系数(0-25.599)
} GO_MotorCmd_t;
/// @brief 电机反馈结构体
typedef struct
{
unsigned char motor_id; // 电机ID
unsigned char mode; // 0:空闲 1:FOC控制 2:电机标定
int Temp; // 温度
int MError; // 错误码
float T; // 当前实际电机输出力矩(电机本身的力矩)(Nm)
float W; // 当前实际电机速度(电机本身的速度)(rad/s)
float Pos; // 当前电机位置(rad)
int correct; // 接收数据是否完整(1完整，0不完整)
int footForce; // 足端力传感器原始数值
typedef struct {
unsigned short id;
unsigned short mode;
uint16_t correct;
int MError;
int Temp;
float tar_pos;
float tar_w;
float T;
float W;
float Pos;
float angle;
uint16_t calc_crc;
uint32_t timeout; // 通讯超时 数量
uint32_t bad_msg; // CRC校验错误 数量
RIS_MotorData_t motor_recv_data; // 电机接收数据结构体
int footForce;
uint8_t buffer[17];
uint8_t Rec_buffer[16];
ControlData_t motor_send_data;
} GO_MotorData_t;
}GO_Motorfield;
void GO_M8010_init(void);
void GO_M8010_send_data(UART_HandleTypeDef *huart,int id, int rev,float T,float W,float Pos,float K_P,float K_W);
void modify_data(GO_MotorCmd_t *motor_s);
void extract_data(GO_MotorData_t *motor_r);
const GO_MotorData_t *get_GO_measure_point(void);
void GO_M8010_send_data( GO_MotorCmd_t *cmd);
void USART6_RxCompleteCallback(void );
void uartTxCB(UART_HandleTypeDef *huart);
void gogogo(void );
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);
GO_Motorfield* getGoPoint(uint8_t id);
#ifdef __cplusplus
}
#endif
#endif /*__GO_M8010_6_H */

4
User/task/cmd_task.c
View File

@ -45,9 +45,9 @@ void Task_cmd(void *argument){
osKernelUnlock(); /* 同上 解锁RTOS内核 */
osMessageQueueReset(task_runtime.msgq.cmd.up_ctrl);
osMessageQueueReset(task_runtime.msgq.cmd.cmd_ctrl_t);
osMessageQueuePut(task_runtime.msgq.cmd.up_ctrl,&cmd,0,0);
osMessageQueuePut(task_runtime.msgq.cmd.cmd_ctrl_t,&cmd,0,0);
tick += delay_tick; /*计算下一个唤醒时刻*/
osDelayUntil(tick); /*绝对延时 等待下一个唤醒时刻 */

2
User/task/init.c
View File

@ -85,7 +85,7 @@ void Task_Init(void *argument) {
osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL);
task_runtime.msgq.cmd.raw.nuc =
osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL);
task_runtime.msgq.cmd.up_ctrl =
task_runtime.msgq.cmd.cmd_ctrl_t =
osMessageQueueNew(3u,sizeof(CMD_t), NULL);
osKernelUnlock();

5
User/task/up_task.c
View File

@ -68,7 +68,7 @@ void Task_up(void *argument)
//
// UP_control(&UP,&UP_CAN_out,&up_cmd);
UP_control(&UP,&UP_CAN_out,&up_cmd);
UP.motor_target .go_shoot =aaa;
// UP.motor_target .M2006_angle =bbb ;
@ -78,7 +78,6 @@ void Task_up(void *argument)
// UP.motor_target .go_spin =ddd;
//
ALL_Motor_Control(&UP,&UP_CAN_out);
osDelay(1);
@ -91,7 +90,7 @@ void Task_up(void *argument)
/*can上设备数据获取*/
osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
osMessageQueueGet(task_runtime.msgq.cmd .up_ctrl , &up_cmd, NULL, 0);
osMessageQueueGet(task_runtime.msgq.cmd .cmd_ctrl_t , &up_cmd, NULL, 0);
/*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/
osKernelLock();

2
User/task/user_task.h
View File

@ -67,7 +67,7 @@ typedef struct {
osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/
}raw;
osMessageQueueId_t up_ctrl;
osMessageQueueId_t cmd_ctrl_t;
osMessageQueueId_t status;
} cmd;
/* can任务放入、读取，电机或电容的输入输出 */