BoooomFOC_STSPIN32G4_EVB/BoooomCTL/Data/Angle/Angle.c

//
// Created by ZK on 2023/3/14.
//

#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_AS5600() {
//    printf("Test");

    return 0.0f;
}

bool Data_Init(tData *data) {
    switch (data->Angle.EncoderModel) {
        case MT6816:
            data->Angle.getAngle = getAngle_MT6816;
            break;
        case AS5600:
            data->Angle.getAngle = getAngle_AS5600;
            break;
    }
    return 0;
}