//
// Created by hu123456 on 2024/1/16.
//

#ifndef FOC_N_FOC_H
#define FOC_N_FOC_H

#include "mymain.h"

typedef struct
{
    float ZT_KP;
    float ZT_KI;
    float ZT_KD;
    float V_KP;
    float V_KI;
    float V_KD;
    float ZT_ErrorPrev;
    float V_Prev;
    float V_Pid_IPrev;
    float V_Error_Prev;
    float V_Out_Prev;
    float zero_electric_angle;
    double Angle_Prev;
    unsigned long Time_Prev;
}FOCTypeDef;
extern FOCTypeDef FOCStruct_X;
extern FOCTypeDef FOCStruct_Y;

int alignSensor(unsigned char moto,FOCTypeDef * FOCStruct);
void PID_Iint(void);
double _normalizeAngle(float angle);
double LPF_velocity(double x,FOCTypeDef *FOCStruct);
void speed_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float  V_Target);
void Zitai_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float  ZT_Error);
void Angel_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float Angle_target);

void PWM_Start();
void moto_Init();
#endif //FOC_N_FOC_H