Boooom/BoooomFOC_STSPIN32G4_EVB
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

完成速度、电流闭环

Browse Source
This commit is contained in:
BoomPC
2024-05-17 00:43:58 +08:00
parent d531b7cc09
commit f404369a32
11 changed files with 507 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files

148
BoooomCTL/Data/Angle/Angle.c
View File

@@ -3,14 +3,152 @@
//
#include "Angle.h"
#include "usbd_cdc_if.h"
#include "Communication.h"
#define RD_REG_3 0x8300 // 0xffff//
#define RD_REG_4 0x8400 // 0x0000//
//#define POLE_PAIRS 7 //小电机
//双天电机：14，极飞A12：21
//#define POLE_PAIRS 21//大电机
#define POLE_PAIRS 20//大电机
#define LINE_NUM 16384
#define AA_LINE ((float32_t)LINE_NUM / (float32_t)POLE_PAIRS) // 2340.57
#define AA_ANGLE ((float32_t)LINE_NUM / 360.0f) // 45.51
#define BB_ANGLE ((float32_t)AA_LINE / 360.0f) // 6.50
#define ANGLE_RATIO (float32_t)(360.0f / 16384.0f)
//#define OFFSET 680 //小电机
//#define OFFSET 730 //大电机
//#define OFFSET 1020 //大电机
// uint16_t OFFSET_0 = 280;
//uint16_t OFFSET_0 = -10000;
float32_t OFFSET=3000;
#define OFFSET_1 1100
//#define OFFSET_0 30
//#define OFFSET_1 300
uint16_t sendBuf[2] = {RD_REG_3, RD_REG_4};
uint32_t timeout;
float32_t speed, speedFilt, filt = 0.01f;
uint16_t cycleNum;
struct ENCODER_Type encoder0 = {
.hspi = &hspi1,
.CS_Port = GPIOD,
.CS_Pin = GPIO_PIN_2,
.sendBuf = {RD_REG_3, RD_REG_4},
// .offset = OFFSET_0,
};
uint8_t SPI_TransmitReceive(SPI_HandleTypeDef *hspi, uint16_t TxData, uint16_t *RxData) {
uint32_t cnt = 0;
while ((hspi->Instance->SR & SPI_SR_TXE) == 0);
hspi->Instance->DR = TxData;
while (cnt < 50) {
if ((hspi->Instance->SR & SPI_SR_RXNE)) {
*RxData = hspi->Instance->DR;
return 0;
}
cnt++;
}
return 1;
}
uint16_t MT_ReadAngle(void) {
HAL_GPIO_WritePin(encoder0.CS_Port, encoder0.CS_Pin, GPIO_PIN_RESET);
timeout = SPI_TransmitReceive(encoder0.hspi, encoder0.sendBuf[0], &encoder0.recvBuf[0]);
HAL_GPIO_WritePin(encoder0.CS_Port, encoder0.CS_Pin, GPIO_PIN_SET);
for (int i = 0; i < 2; i++);
HAL_GPIO_WritePin(encoder0.CS_Port, encoder0.CS_Pin, GPIO_PIN_RESET);
timeout = SPI_TransmitReceive(encoder0.hspi, encoder0.sendBuf[1], &encoder0.recvBuf[1]);
HAL_GPIO_WritePin(encoder0.CS_Port, encoder0.CS_Pin, GPIO_PIN_SET);
/*处理原始数据并转换为机械角度*/
encoder0.angleVal = ((encoder0.recvBuf[0] & 0x00FF) << 6) + ((encoder0.recvBuf[1] & 0x00FF) >> 2);
encoder0.angle_360 = (float32_t) encoder0.angleVal * ANGLE_RATIO;
/*软件记圈*/
if (encoder0.lastAngle > 9000 && encoder0.angleVal < 7000)
encoder0.cycleNum++;
else if (encoder0.lastAngle < 7000 && encoder0.angleVal > 9000)
encoder0.cycleNum--;
/*记速*/
// encoder->angleArr[encoder->cnt][0] = encoder->angleVal;
// encoder->angleArr[encoder->cnt][1] = encoder->cycleNum;
// int32_t temp1 = encoder->angleArr[encoder->cnt][0] - encoder->angleArr[(encoder->cnt +1) % 8][0];
// int32_t temp2 = (encoder->angleArr[encoder->cnt][1] - encoder->angleArr[(encoder->cnt +1) % 8][1]) << 14;
// encoder->speed = (float32_t)(temp1 + temp2) ;// / 32.0f;
// int32_t temp1 = 0;
// int32_t temp2 = 0;
// int32_t sum = 0;
// for(uint8_t i = 0; i < 8 - 1; i++)
// {
// temp1 = encoder->angleArr[i + 1][0] - encoder->angleArr[i][0];
// temp2 = (encoder->angleArr[i + 1][1] - encoder->angleArr[i][1]) << 14;
// sum += (temp1 + temp2) ;// / 32.0f;
// }
// encoder->speed = (float32_t)(sum) / 7.0f;// / 32.0f;
// encoder->cnt ++;
// encoder->cnt &= 0x07;
encoder0.lastAngle = encoder0.angleVal;
return encoder0.angleVal;
}
float32_t GetSpeed(uint16_t angleVal) {
static int32_t lastAngle, lastCycle;
speed = (float32_t) (angleVal - lastAngle + ((encoder0.cycleNum - lastCycle) << 14));
lastAngle = angleVal;
lastCycle = encoder0.cycleNum;
speedFilt = speed * filt + (1.0f - filt) * speedFilt;
return speedFilt;
// return speed;
}
float32_t getAngle_MT6816() {
// float32_t temp = (float32_t)angleVal + (float32_t)encoder0.offset;
// OFFSET+=0.01f;
// usb_printf("%d\r\n", OFFSET);
// if (OFFSET >= 16384.0f){OFFSET= 0.0f;}
float32_t temp = (float32_t) MT_ReadAngle()+16274;
// SendCurrent_Vofa(temp,encoder0.eAngle_360, 0);
while (temp > AA_LINE) {
temp -= AA_LINE;
}
encoder0.eAngle_360 = temp / BB_ANGLE;
// SendCurrent_Vofa(OFFSET,encoder0.eAngle_360, 0);
return encoder0.eAngle_360;
}
//#define EncoderName MT6816
float32_t getAngle_MT6816() {
// printf("Test");
return M_PI;
}
//float32_t getAngle_MT6816() {
//// printf("Test");
//
// return M_PI;
//}
float32_t getAngle_AS5600() {
// printf("Test");

43
BoooomCTL/Data/Angle/Angle.h
View File

@@ -5,7 +5,22 @@
#ifndef BOOOOMFOC_STSPIN32G4_EVB_ANGLE_H
#define BOOOOMFOC_STSPIN32G4_EVB_ANGLE_H
#include "APP_Main.h"
//#include "APP_Main.h"
#include "arm_math.h"
#include "main.h"
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include "adc.h"
#include "dma.h"
#include "i2c.h"
#include "opamp.h"
#include "spi.h"
#include "tim.h"
#include "usb_device.h"
#include "gpio.h"
typedef enum {
MT6816,
@@ -24,6 +39,9 @@ typedef struct {
float32_t (*Angle_Init)();
} tData;
uint16_t MT_ReadAngle(void);
float32_t GetSpeed(uint16_t angleVal);
float32_t getAngle_MT6816();
float32_t getAngle_AS5600();
@@ -63,4 +81,27 @@ typedef struct sEncoder {
float snap_threshold;
} tEncoder;
struct ENCODER_Type {
SPI_HandleTypeDef *hspi;
GPIO_TypeDef *CS_Port;
uint16_t CS_Pin;
uint16_t sendBuf[2];
uint16_t recvBuf[2];
uint16_t offset;
uint16_t angleVal, lastAngle;
int32_t cycleNum;
float32_t angle_360;
float32_t eAngle_360;
// int32_t angleArr[32][2];
// uint8_t cnt;
float32_t speed;
float32_t filt;
float32_t speedFilt;
};
#endif //BOOOOMFOC_STSPIN32G4_EVB_ANGLE_H