工程文件参考——STM32F4+基于螺旋理论的指数积公式驱动

某先森不吃鱼

已于 2023-06-16 21:13:35 修改

阅读量277

点赞数

分类专栏：现代机器人学——螺旋理论文章标签： stm32 嵌入式硬件单片机

于 2023-06-16 21:07:02 首次发布

本文链接：https://blog.csdn.net/qq_23297513/article/details/131253511

版权

现代机器人学——螺旋理论专栏收录该内容

4 篇文章 2 订阅

订阅专栏

该文提供了基于STM32F4芯片的指数积公式驱动实现，利用q15格式进行数学计算。代码包括了POE_Formula.h和POE_Formula.c文件，用于创建和初始化矩阵，以及执行计算。MyMaths.h和MyMaths.c文件包含了辅助的矩阵操作函数，如矩阵初始化、设置和打印。文章展示了如何进行旋转矩阵的生成和应用，用于坐标变换。

摘要由CSDN通过智能技术生成

文章目录

代码实现

# 前言

本文主要提供使用F4系列芯片构建指数积公式驱动，以供参考

数学计算依靠arm_dsp运行
存储与计算皆使用q15格式，如果需要更高精度，可自行更改为f32格式

GITHUB
RBTCRTL_20230615

代码实现

POE_Formula.h

/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __POE_FORMULA_H__
#define __POE_FORMULA_H__
/* =================================================================================
File name:       __POE_Formula_H__
Author: Mr.NoFish
===================================================================================*/

#ifdef __cplusplus
extern "C" {
#endif

/* Includes ------------------------------------------------------------------*/
#include "MyMaths.h"

typedef struct POE_Formula_ POE_Formula;

typedef void (*POEFfptrInit)(POE_Formula*, q15_t, q15_t*);
typedef void (*POEFfptrCalc)(POE_Formula*, q15_t);
typedef void (*POEFfptrSet)(POE_Formula*, q15_t);

struct POE_Formula_
{
	q15_t theta;
	
	uint8_t axis;//0:x, 1:y, 2:z, 
	
	q15_t pos[3];
	
	arm_matrix_instance_q15* POE_Mat;
	
	//interface for function
	POEFfptrInit init;
	POEFfptrCalc calc;
};
 
POE_Formula* new_POE_Formula(void);
void delete_POE_Formula(POE_Formula* const pPOEFObj);


void POE_Formula_init(POE_Formula* const pPOEFObj, q15_t axis, q15_t* pos);
void POE_Formula_calc(POE_Formula* const pPOEFObj, q15_t theta);
void mat_q15_print(arm_matrix_instance_q15* const pMatObj);

#endif

POE_Formula.c

#include "POE_Formula.h"

POE_Formula* new_POE_Formula(void)
{
	POE_Formula* pObj = NULL;
	pObj = (POE_Formula*)malloc(sizeof(POE_Formula));
	if (pObj == NULL)
	{
		printf("WARN: POE_Formula initialization failed.\r\n");
		return NULL;
	}
	
	pObj->init = POE_Formula_init;
	pObj->calc = POE_Formula_calc;
	
	pObj->POE_Mat = new_mat_q15(4, 4);
	
	printf("INFO: POE_Formula initialization succeeded.\r\n");
	
	return pObj;
}

void delete_POE_Formula(POE_Formula* const pPOEFObj)
{
	
}

void POE_Formula_init(POE_Formula* const pPOEFObj, q15_t axis, q15_t* pos)
{
	q15_t pState[9];
	pPOEFObj->axis = axis;
	memcpy(pPOEFObj->pos, pos, 3*sizeof(q15_t));
	memset(pPOEFObj->POE_Mat->pData, 0, 16*sizeof(q15_t));
}

void POE_Formula_calc(POE_Formula* const pPOEFObj, q15_t theta)
{
	pPOEFObj->theta = theta;

	q15_t temp_sin, temp_cos = 0;
	temp_cos = arm_cos_q15(theta);
	temp_sin = arm_sin_q15(theta);
	
	mat_q15_set(pPOEFObj->POE_Mat, pPOEFObj->axis % 3, pPOEFObj->axis % 3, 0x7fff);
	mat_q15_set(pPOEFObj->POE_Mat, (pPOEFObj->axis + 1) % 3, (pPOEFObj->axis + 1) % 3, temp_cos);
	mat_q15_set(pPOEFObj->POE_Mat, (pPOEFObj->axis + 2) % 3, (pPOEFObj->axis + 2) % 3, temp_cos);
	mat_q15_set(pPOEFObj->POE_Mat, (pPOEFObj->axis + 1) % 3, (pPOEFObj->axis + 2) % 3, -temp_sin);
	mat_q15_set(pPOEFObj->POE_Mat, (pPOEFObj->axis + 2) % 3, (pPOEFObj->axis + 1) % 3, temp_sin);
	
	mat_q15_set(pPOEFObj->POE_Mat, 0, 3, 
		pPOEFObj->pos[0]
		-(q15_t)((((q31_t)mat_q15_get(pPOEFObj->POE_Mat, 0, 0)) * pPOEFObj->pos[0])>>15)
		-(q15_t)((((q31_t)mat_q15_get(pPOEFObj->POE_Mat, 0, 1)) * pPOEFObj->pos[1])>>15)
		-(q15_t)((((q31_t)mat_q15_get(pPOEFObj->POE_Mat, 0, 2)) * pPOEFObj->pos[2])>>15)
		);
	mat_q15_set(pPOEFObj->POE_Mat, 1, 3, 
		pPOEFObj->pos[1]
		-(q15_t)((((q31_t)mat_q15_get(pPOEFObj->POE_Mat, 1, 0)) * pPOEFObj->pos[0])>>15)
		-(q15_t)((((q31_t)mat_q15_get(pPOEFObj->POE_Mat, 1, 1)) * pPOEFObj->pos[1])>>15)
		-(q15_t)((((q31_t)mat_q15_get(pPOEFObj->POE_Mat, 1, 2)) * pPOEFObj->pos[2])>>15)
		);
	mat_q15_set(pPOEFObj->POE_Mat, 2, 3, 
		pPOEFObj->pos[2]
		-(q15_t)((((q31_t)mat_q15_get(pPOEFObj->POE_Mat, 2, 0)) * pPOEFObj->pos[0])>>15)
		-(q15_t)((((q31_t)mat_q15_get(pPOEFObj->POE_Mat, 2, 1)) * pPOEFObj->pos[1])>>15)
		-(q15_t)((((q31_t)mat_q15_get(pPOEFObj->POE_Mat, 2, 2)) * pPOEFObj->pos[2])>>15)
		);

	mat_q15_set(pPOEFObj->POE_Mat, 3, 3, 0x7fff);

	return;
}

MyMaths.h

/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __MYMATHS_H__
#define __MYMATHS_H__
/* =================================================================================
File name:       __MYMATHS_H__
Author: Mr.NoFish
===================================================================================*/

#ifdef __cplusplus
extern "C" {
#endif

#define M_2_PI 		6.28318530718
#define M_PI 		3.14159265359
#define M_PI_2 	1.57079632679
#define M_PI_4	0.785398163397

/* Includes ------------------------------------------------------------------*/

#include "main.h"
#include "arm_math.h"

arm_matrix_instance_q15* new_mat_q15(uint16_t nRows, uint16_t nColumns);
void delete_mat_q15(arm_matrix_instance_q15* const pMatObj);


void mat_q15_set(arm_matrix_instance_q15* const pMatObj, uint16_t nRows, uint16_t nColumns, q15_t value);
q15_t mat_q15_get(arm_matrix_instance_q15* const pMatObj, uint16_t nRows, uint16_t nColumns);
void mat_q15_print(arm_matrix_instance_q15* const pMatObj);

q15_t my_float_to_q15(float Src);
float my_q15_to_float(q15_t Src);


#endif

MyMaths.c

#include "MyMaths.h"


arm_matrix_instance_q15* new_mat_q15(uint16_t nRows, uint16_t nColumns)
{
	arm_matrix_instance_q15* pObj = NULL;
	q15_t* pData = NULL;
	
	pObj = (arm_matrix_instance_q15*)malloc(sizeof(arm_matrix_instance_q15));
	pData = (q15_t*)malloc(sizeof(q15_t)* nRows * nColumns);
	memset(pData, 0, sizeof(q15_t)* nRows * nColumns);
	
	if (pObj == NULL)
	{
		printf("WARN: Matrix memory allocation failed.(pObj)\r\n");
		return NULL;
	}

	if (pData == NULL)
	{
		printf("WARN: Matrix memory allocation failed.(pData)\r\n");
		return NULL;
	}
	
	arm_mat_init_q15(pObj, nRows, nColumns, pData);
	printf("INFO: Matrix memory allocation succeeded.\r\n");
	return pObj;
}

void delete_mat_q15(arm_matrix_instance_q15* const pMatObj)
{

}

void mat_q15_set(arm_matrix_instance_q15* const pMatObj, uint16_t nRows, uint16_t nColumns, q15_t value)
{
	pMatObj->pData[nRows * pMatObj->numCols + nColumns] = value;
}

q15_t mat_q15_get(arm_matrix_instance_q15* const pMatObj, uint16_t nRows, uint16_t nColumns)
{
	return pMatObj->pData[nRows * pMatObj->numCols + nColumns];
}

void mat_q15_print(arm_matrix_instance_q15* const pMatObj)
{
	uint16_t i, j = 0;
	for(i = 0; i<pMatObj->numRows; i++)
	{
		for(j = 0; j<pMatObj->numRows; j++)
			printf("%+.4f ", my_q15_to_float(mat_q15_get(pMatObj, i, j)));
		printf("\r\n");
	}
}

q15_t my_float_to_q15(float Src)
{
	q15_t temp;
	arm_float_to_q15(&Src, &temp, 1);
	return temp;
}

float my_q15_to_float(q15_t Src)
{
	float temp;
	arm_q15_to_float(&Src, &temp, 1);
	return temp;
}