526b689693
1/ Added Spi that used to read data from AS5045b position sensor 2/ Added Base FOC code to project 3/ Added Standart DSP lib with optimazes sincos cals Need for FOC
147 lines
3.4 KiB
C
147 lines
3.4 KiB
C
#include "main.h"
|
|
#pragma once
|
|
|
|
#define DEF_MAX_CURRENT (1.00f)
|
|
|
|
typedef enum {
|
|
|
|
MOTOR_STATE_IDLE = 0,
|
|
|
|
MOTOR_STATE_GOTO_ZERO = 1,
|
|
MOTOR_STATE_GOTO_ZERO_FINISH = 2,
|
|
|
|
MOTOR_STATE_STABILIZATION = 3,
|
|
|
|
MOTOR_STATE_ERROR = 4,
|
|
|
|
MOTOR_STATE_TURN_OFF = 5,
|
|
MOTOR_STATE_TURN_OFF_FINISH = 6,
|
|
|
|
MOTOR_STATE_EMERGENCY_BRAKE = 7,
|
|
|
|
MOTOR_STATE_FLUX_ALIGN =10,
|
|
|
|
MOTOR_STATE_TEST_PID =20,
|
|
|
|
MOTOR_STATE_TEST_MOTION =30,
|
|
|
|
} ENUM_MOTOR_STATES;
|
|
|
|
#pragma pack(push,1)
|
|
|
|
typedef struct {
|
|
uint32_t u32CpuID;
|
|
float f32TetaCorrection;
|
|
float f32MinAngle_deg;
|
|
float f32MaxAngle_deg;
|
|
float f32IcogCompCoef;
|
|
float f32IaccelCompFactor;
|
|
float f32PositionZero_deg;
|
|
float f32PidPositionKd;
|
|
float f32PidPositionKi;
|
|
float f32PidPositionKp;
|
|
float f32PidPositionTfilt_d;
|
|
float f32UpRateZero;
|
|
float f32DownRateZero;
|
|
float f32UpRateStab;
|
|
float f32DownRateStab;
|
|
} TPARAMS_USED_BY_SERVO;
|
|
|
|
#pragma pack(pop)
|
|
|
|
|
|
extern TPARAMS_USED_BY_SERVO stUsedParameters;
|
|
|
|
|
|
extern uint32_t CPU_ID;
|
|
/*TEST pins define section*/
|
|
#define TESTPIN_ENABLE 0
|
|
#define TESTPIN_UART_ISR_ENABLE (1)
|
|
//#define TEST_UART 0
|
|
#define FSAMPLE_FAST (10000)
|
|
#define TSAMPLE_FAST (1.0f/FSAMPLE_FAST)
|
|
#define FSAMPLE_SLOW (1000)
|
|
#define TSAMPLE_SLOW (1.0f/FSAMPLE_SLOW)
|
|
static inline float ISR_TIME_CALC(float start,float stop)
|
|
{
|
|
static float systemTimeStep = -1.f;
|
|
if(systemTimeStep <0.f){
|
|
systemTimeStep=1.f/SystemCoreClock;
|
|
return 0;
|
|
}
|
|
if (start > stop)
|
|
{
|
|
return ((start- stop)*(systemTimeStep));
|
|
}
|
|
else
|
|
{
|
|
uint32_t counter = ((0xFFFFFF - stop) + start) + 1;
|
|
return ( (counter)*(systemTimeStep));
|
|
}
|
|
}
|
|
|
|
static inline uint16_t GET_PWM_Period(uint16_t ID)
|
|
{
|
|
return((uint16_t)LL_TIM_GetAutoReload(TIM1));
|
|
}
|
|
static inline void PWM_OUT_ENABLE(uint16_t ID)
|
|
{
|
|
LL_GPIO_SetOutputPin(EN_U_GPIO_Port,EN_U_Pin);
|
|
LL_GPIO_SetOutputPin(EN_W_GPIO_Port,EN_W_Pin);
|
|
LL_GPIO_SetOutputPin(EN_V_GPIO_Port,EN_V_Pin);
|
|
LL_TIM_EnableAllOutputs(TIM1);
|
|
}
|
|
static inline void PWM_OUT_DISABLE(uint16_t ID)
|
|
{
|
|
LL_GPIO_ResetOutputPin(EN_U_GPIO_Port,EN_U_Pin);
|
|
LL_GPIO_ResetOutputPin(EN_W_GPIO_Port,EN_W_Pin);
|
|
LL_GPIO_ResetOutputPin(EN_V_GPIO_Port,EN_V_Pin);
|
|
LL_TIM_DisableAllOutputs(TIM1);
|
|
}
|
|
static inline void PWM_COMPARE_SET(uint16_t ID,uint16_t A,uint16_t B,uint16_t C)
|
|
{
|
|
LL_TIM_OC_SetCompareCH1(TIM1, A);
|
|
LL_TIM_OC_SetCompareCH2(TIM1, B);
|
|
LL_TIM_OC_SetCompareCH3(TIM1, C);
|
|
|
|
}
|
|
static uint16_t hw_err_state = 0;
|
|
static inline uint16_t GET_HWfault_state(uint16_t ID)
|
|
{
|
|
if(hw_err_state) LL_TIM_DisableIT_BRK(TIM1);
|
|
return hw_err_state;
|
|
}
|
|
static inline void CLEAR_HWfault(uint16_t ID)
|
|
{
|
|
hw_err_state = 0;
|
|
LL_TIM_EnableIT_BRK(TIM1);
|
|
}
|
|
|
|
|
|
static inline uint32_t GET_ADC_data(uint16_t ID,uint16_t *ADC_array,uint16_t *error_code)
|
|
{
|
|
*error_code = 0;
|
|
uint32_t adc_wait1 = 0;
|
|
while((!LL_ADC_IsActiveFlag_JEOS(ADC1))){
|
|
if(adc_wait1++ > 1000) return (*error_code = 0x1);
|
|
};
|
|
|
|
/*Channel 1 ADC 1- Current A*/
|
|
ADC_array[0] = LL_ADC_INJ_ReadConversionData12(ADC1,LL_ADC_INJ_RANK_1);
|
|
/*Channel 2 ADC 1- Current B*/
|
|
ADC_array[2] = LL_ADC_INJ_ReadConversionData12(ADC1,LL_ADC_INJ_RANK_2);
|
|
/*Channel 4 ADC 1 - Vdc bs*/
|
|
ADC_array[5] = LL_ADC_INJ_ReadConversionData12(ADC1,LL_ADC_INJ_RANK_4);
|
|
/*Clear flags*/
|
|
LL_ADC_ClearFlag_JEOS(ADC1);
|
|
return (0x0);
|
|
|
|
}
|
|
static inline uint16_t GET_ENCODER_counter(uint16_t ID)
|
|
{
|
|
return ( LL_TIM_GetCounter(TIM3));
|
|
}
|
|
static inline void SET_ENCODER_counter(uint16_t ID,uint16_t value)
|
|
{
|
|
LL_TIM_SetCounter(TIM3, value);
|
|
}
|