caiyu/STM32G4-DRV8301-FOC
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
30de808b4c
STM32G4-DRV8301-FOC/FOC_Related/FOC/FOC.c

451 lines
14 KiB
C
Raw Normal View History

files upload
2024-07-22 13:55:29 +08:00
#include "FOC.h"
#include "math.h"
#include "as5600.h"
#include "main.h"
#include "control.h"
#include "tim.h"
#include "pid.h"
#include "adc.h"
#include "usart.h"
#include "string.h"
#include "stdio.h"
#include "adc.h"
#include "LowPass_Filter.h"
extern int PP;
extern int DIR;
extern float voltage_limit;
extern float voltage_power_supply;
extern float zero_electric_angle;
extern float Ualpha, Ubeta;
extern float Ua, Ub, Uc;
extern float dc_a, dc_b, dc_c;
extern float Sensor_Angle;
extern float Sensor_Speed;
extern uint32_t adc1_Ia, adc1_Ib, adc1_Ic;
extern uint32_t adc2_Ia, adc2_Ib, adc2_Ic;
extern float current1_A;
extern float current1_B;
extern float current1_C;
extern float volts_to_amps_ratio;
extern float gain_a, gain_b, gain_c;
extern float offset_ia, offset_ib, offset_ic;
extern float i_alpha;
extern float i_beta;
extern float id;
extern float iq;
extern float id_ref;
extern float iq_ref;
extern float Ud;
extern float Uq_new;
uint16_t filtered_ADC1_Ia;
uint16_t filtered_ADC1_Ib;
uint16_t filtered_ADC1_Ic;
/*******************************************
SysTick<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>(ģ<EFBFBD><EFBFBD>micros<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>)<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD>ע<EFBFBD><EFBFBD>
1.<EFBFBD><EFBFBD>0xFFFFFF<EFBFBD><EFBFBD>0ѭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2.<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD>delay<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*******************************************/
void Systick_CountMode(void)
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>϶<EFBFBD>ͨ<EFBFBD>õij<C3B5>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SysTick->LOAD = 0xFFFFFF; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>ʼ
SysTick->VAL = 0; // <20><><EFBFBD>յ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_ENABLE_Msk; // ʹ<><CAB9> SysTick <20><>ʱ<EFBFBD><CAB1>
}
/*******************************************
<EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>⣺
<EFBFBD><EFBFBD>ע<EFBFBD><EFBFBD>
1. <EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD> = <EFBFBD><EFBFBD>е<EFBFBD>Ƕ<EFBFBD> * <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2._electricalAngle = shaft_angle * pole_pairs
*******************************************/
float _electricalAngle()
{
return _normalizeAngle((float)(DIR * PP) * AS5600_GetAngle2PI() - zero_electric_angle);
}
/*******************************************
<EFBFBD>Ƕȹ<EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
<EFBFBD><EFBFBD>ע<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ù<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>ۼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*******************************************/
float _normalizeAngle(float angle)
{
float a = fmod(angle, 2 * PI);
return a >= 0 ? a : (a + 2 * PI);
// <20><>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD><CABD>condition ? expr1 : expr2
// <20><><EFBFBD><EFBFBD>,conditionʽҪ<CABD><D2AA>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C>򷵻<EFBFBD> expr1 <20><>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD>Խ<EFBFBD><D4BD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>if-else<73>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD>ʽ
}
/*******************************************
PWMռ<EFBFBD>ձ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㲢<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
*******************************************/
void setPWM(float Ua, float Ub, float Uc)
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѹ<EFBFBD>޷<EFBFBD>
Ua = _constrain(Ua, 0.0f, voltage_limit);
Ub = _constrain(Ub, 0.0f, voltage_limit);
Uc = _constrain(Uc, 0.0f, voltage_limit);
// ռ<>ձȼ<D5B1><C8BC>㣬<EFBFBD><E3A3AC><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD>
dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);
// <20><><EFBFBD><EFBFBD>1 6·PWMռ<4D>ձ<EFBFBD><D5B1><EFBFBD><EFBFBD><EFBFBD>
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, dc_a * 5500);
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, dc_b * 5500);
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, dc_c * 5500);
//__HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_1, dc_a * 5500);
//__HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_2, dc_b * 5500);
//__HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_3, dc_c * 5500);
}
/*******************************************
<EFBFBD><EFBFBD><EFBFBD>ؿ<EFBFBD><EFBFBD><EFBFBD>:
*******************************************/
void setTorque(float Uq, float angle_el)
{
// <20><><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD><DEB7><EFBFBD>
Uq = _constrain(Uq, -voltage_power_supply / 2, voltage_power_supply / 2);
// <20>Ƕȹ<C7B6>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
angle_el = _normalizeAngle(angle_el);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>任
Ualpha = -Uq * sin(angle_el);
Ubeta = Uq * cos(angle_el);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>任
Ua = Ualpha + voltage_power_supply / 2;
Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
Uc = (-Ualpha - sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2;
// PWM<57><4D>ֵ
setPWM(Ua, Ub, Uc);
}
/*******************************************
FOC<EFBFBD><EFBFBD><EFBFBD>Ƴ<EFBFBD>ʼ<EFBFBD><EFBFBD>:
*******************************************/
void FOC_Init(float power_supply)
{
voltage_power_supply = power_supply; // <20><>Դ<EFBFBD><D4B4>ѹ
// <20>˴<EFBFBD><CBB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD>Ӳ<EFBFBD><D3B2><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>
// NVIC_Config();<3B>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ȼ<EFBFBD><C8BC><EFBFBD><EFBFBD><EFBFBD>
PID_init(); // <20><>ʼ<EFBFBD><CABC>pid<69><64><EFBFBD><EFBFBD>
}
/*******************************************
FOC<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD>ʼ<EFBFBD><EFBFBD>:
*******************************************/
void FOC_AS5600_Init(int _PP, int _DIR)
{
// ֵ<><D6B5><EFBFBD>ݡ<EFBFBD>
PP = _PP;
DIR = _DIR;
// setTorque(3, _2PI);
//
//HAL_Delay(3000);
setTorque(0, _2PI);
zero_electric_angle = _electricalAngle(); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕȡ<C7B6>
Sensor_Speed = AS5600_Get_Speed(); // <20>ٶȳ<D9B6>ʼֵ<CABC><D6B5><EFBFBD>㡣
}
/***********************************************
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕȿ<EFBFBD><EFBFBD><EFBFBD>:(rad)
***********************************************/
void Set_Angle(float Angle)
{
// AS5600<30>Ƕȶ<C7B6>ȡ<EFBFBD><C8A1>
Sensor_Angle = AS5600_GetAngle();
// <20>Ƕȿ<C7B6><C8BF>ơ<EFBFBD>
Angle_Out = Angle_Control((Angle - DIR * Sensor_Angle) * 180.0f / PI);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD><DEB7><EFBFBD>
Moment_limiting();
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ء<EFBFBD>
setTorque(Angle_Out, _electricalAngle());
// <20>Ƕȴ<C7B6><C8B4>ڴ<EFBFBD>ӡ<EFBFBD><D3A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// printf("%.2f,%.2f\n",Sensor_Angle,Angle_target);
}
/***********************************************
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȿ<EFBFBD><EFBFBD><EFBFBD>:(rad/s)
***********************************************/
void Set_Speed(float Speed)
{
// AS5600<30><30>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݡ<EFBFBD>
Sensor_Speed = AS5600_Get_Speed();
// <20>ٶ<EFBFBD>PID<49><44><EFBFBD><EFBFBD>
Speed_Out = Speed_Control(Speed - Sensor_Speed);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD>
Moment_limiting();
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؿ<EFBFBD><D8BF><EFBFBD>
setTorque(Speed_Out, _electricalAngle());
// <20>ٶȴ<D9B6><C8B4>ڴ<EFBFBD>ӡ<EFBFBD><D3A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// printf("%.2f,%.2f\n",Sensor_Speed,Speed_target);
}
/***********************************************
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת:
***********************************************/
void Open_Loop_Control(float Uq, float speed)
{
// <20><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>
// char buffer[50]; // <20><><EFBFBD>ڴ洢ת<E6B4A2><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7><EFBFBD>
// int length = 0;
float angle_el; // <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
while (1)
{
// Read_ADC1_Values();
Send_ADC1_Values(&huart1); // <20><><EFBFBD><EFBFBD> ADC1 <20><>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
// <20><><EFBFBD>µ<EFBFBD><C2B5>Ƕ<EFBFBD>
angle_el += speed;
angle_el = _normalizeAngle(angle_el);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
setTorque(Uq, angle_el);
// <20><>ȡ<EFBFBD><C8A1>ǰADCֵ
// <20><><EFBFBD>Դ<EFBFBD>ӡ<EFBFBD><D3A1>ȷ<EFBFBD><C8B7>ADCֵ<43>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>ȷ<EFBFBD><C8B7>ȡ
// HAL_UART_Transmit(&huart1, "Toggle LED0!\r\n", sizeof("Toggle LED0!\r\n"),10000);
// HAL_StatusTypeDef HAL_UART_Transmit(UART_HandleTypeDef *huart1, uint8_t *adc1_Ia, sizeof(adc1_Ia), 1);
// length = snprintf(buffer, sizeof(buffer), "ADC1_Ia: %lu\r\n", adc2_Ia);
// HAL_UART_Transmit(&huart1, (uint8_t *)buffer, length, HAL_MAX_DELAY);
// <20><>ʱ<EFBFBD><CAB1>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5>
HAL_Delay(1); // <20><><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ
}
}
/***********************************************
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ADC <EFBFBD><EFBFBD>ƫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĺ<EFBFBD><EFBFBD><EFBFBD>:
***********************************************/
void calibrateOffsets()
{
const int calibration_rounds = 1000;
// <20><><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>ĵ<EFBFBD>ѹ
offset_ia = 0;
offset_ib = 0;
offset_ic = 0;
// <20><><EFBFBD><EFBFBD>1000<30><30>
for (int i = 0; i < calibration_rounds; i++)
{
offset_ia += adc1_Ia * (3.3f / 4095.0f); // <20><>ԭʼֵ<CABC><D6B5><EFBFBD><EFBFBD>ʵ<EFBFBD>ʵ<EFBFBD>ѹֵ<D1B9>ۼӽ<DBBC>offset_ia
offset_ib += adc1_Ib * (3.3f / 4095.0f);
offset_ic += adc1_Ic * (3.3f / 4095.0f);
}
// <20><>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD>
offset_ia = offset_ia / calibration_rounds;
offset_ib = offset_ib / calibration_rounds;
offset_ic = offset_ic / calibration_rounds;
// ʵ<><CAB5>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>500mv<6D><76>Ӧ5A<35><41><EFBFBD><EFBFBD><EFBFBD><EFBFBD>x10<31><30><EFBFBD>ʹﵽ<CDB4>˶<EFBFBD>Ӧ<EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD><EFBFBD>
volts_to_amps_ratio = 1.0f / 0.01f / 10; // 1/(0.01o+10) <20><><EFBFBD>ò<EFBFBD><C3B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŵ<EFBFBD><C5B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>档
// <20><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>
gain_a = volts_to_amps_ratio * -1;
gain_b = volts_to_amps_ratio * -1;
gain_c = volts_to_amps_ratio * -1;
}
/***********************************************
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void Feedback_Current()
{
// <20><><EFBFBD>㻬<EFBFBD><E3BBAC><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD>ʵ<EFBFBD>ѹ<EFBFBD><D1B9>-ƫִ<C6AB><D6B4>ѹ<EFBFBD><D1B9>*<2A><><EFBFBD><EFBFBD>
current1_A = ((filtered_ADC1_Ia * (3.3f / 4095.0f)) - offset_ia) * gain_a;
current1_B = ((filtered_ADC1_Ib * (3.3f / 4095.0f)) - offset_ib) * gain_b;
current1_C = ((filtered_ADC1_Ic * (3.3f / 4095.0f)) - offset_ic) * gain_c;
}
/***********************************************
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؿ<EFBFBD><EFBFBD><EFBFBD>:
***********************************************/
void Current_Speed(float Uq, float speed)
{
float angle_el = 0;
calibrateOffsets(); // У׼<D0A3><D7BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
while (1)
{
// uint8_t Senbuff[] = "Serial Output Message by DMA! \r\n"; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݷ<EFBFBD><DDB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Send_ADC1_Values(&huart1); // <20><><EFBFBD><EFBFBD>1 <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>DMA<4D><41><EFBFBD>ͺ<EFBFBD><CDBA><EFBFBD><EFBFBD><EFBFBD>
angle_el += speed; // <20><><EFBFBD><EFBFBD><EFBFBD>ۼӸ<DBBC><D3B8>µ<EFBFBD><C2B5>Ƕȡ<C7B6>
angle_el = _normalizeAngle(angle_el); // <20>Ƕȹ<C7B6>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Current_Loop(Uq, angle_el); // <20><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//HAL_Delay(1); // <20><>ʱ<EFBFBD><CAB1>
}
}
/***********************************************
<EFBFBD><EFBFBD><EFBFBD>ڷ<EFBFBD><EFBFBD><EFBFBD>ADC1ֵ
<EFBFBD><EFBFBD>ע<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void Send_ADC1_Values(UART_HandleTypeDef *huart)
{
char buffer[100];
int len;
Read_ADC1_Values(); // <20><><EFBFBD>ȶ<EFBFBD>ȡԭʼADC<44><43><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>
// uint8_t Senbuff[] = " DMA OK! \r\n"; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݷ<EFBFBD><DDB7>Ͳ<EFBFBD><CDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>顣
// <20><>ȡ ADC1 <20><>ֵ<EFBFBD><D6B5>
uint16_t new_adc1_Ia = adc1_Ia;
uint16_t new_adc1_Ib = adc1_Ib;
uint16_t new_adc1_Ic = adc1_Ic;
// <20><><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>
filtered_ADC1_Ia = ADC_Filter_Update(&adc1_filter_Ia, new_adc1_Ia);
filtered_ADC1_Ib = ADC_Filter_Update(&adc1_filter_Ib, new_adc1_Ib);
filtered_ADC1_Ic = ADC_Filter_Update(&adc1_filter_Ic, new_adc1_Ic);
Feedback_Current(); // <20><>ȡ<EFBFBD><C8A1><EFBFBD>õķ<C3B5><C4B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20><>ʽ<EFBFBD><CABD><EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ADC1 <20><>ֵת<D6B5><D7AA>Ϊ<EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӡ<EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>
len = snprintf(buffer, sizeof(buffer), "%u, %u, %u\n", filtered_ADC1_Ia, filtered_ADC1_Ib, filtered_ADC1_Ic);
// ͨ<><CDA8><EFBFBD><EFBFBD><EFBFBD>ڷ<EFBFBD><DAB7><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
HAL_UART_Transmit(huart, (uint8_t *)buffer, len, 3); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӡ<EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// HAL_UART_Transmit_DMA(&huart, (uint8_t *)Senbuff, sizeof(Senbuff));//DMA<4D><41><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// HAL_Delay(1);
}
/***********************************************
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void Current_Loop(float Uq, float angle_el)
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA>-<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>
// ʹ<><CAB9>Clarke<6B>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>abc<62><63>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA>-<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD><EFBFBD>i<EFBFBD><69>, i<>£<EFBFBD><C2A3><EFBFBD>
i_alpha = current1_A;
i_beta = (current1_A + 2 * current1_B) / sqrt(3.0f);
// <20><><EFBFBD><EFBFBD>-<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD>ת<EFBFBD><D7AA>Ϊd-q<><71><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>
// ʹ<><CAB9>Park<72>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD>-<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD><EFBFBD>i<EFBFBD><69>, i<>£<EFBFBD>ת<EFBFBD><D7AA>Ϊd-q<><71><EFBFBD><EFBFBD>ϵ<EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD><EFBFBD>id, iq<69><71><EFBFBD><EFBFBD>
id = i_alpha * cos(angle_el) + i_beta * sin(angle_el);
iq = -i_alpha * sin(angle_el) + i_beta * cos(angle_el);
id_ref = 0.0f; // d<><64><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ο<EFBFBD>ֵΪ0
iq_ref = Uq; // q<><71><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ο<EFBFBD>ֵΪĿ<CEAA><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// PID<49><44><EFBFBD>Ƽ<EFBFBD><C6BC><EFBFBD>
Ud = Current_Control(id_ref - id);
Uq_new = Current_Control(iq_ref - iq);
// ʹ<><CAB9><EFBFBD><EFBFBD>Park<72>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>-<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>ĵ<EFBFBD>ѹ
Ualpha = Ud * cos(angle_el) - Uq_new * sin(angle_el);
Ubeta = Ud * sin(angle_el) + Uq_new * cos(angle_el);
// <20><><EFBFBD><EFBFBD>-<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>ĵ<EFBFBD>ѹת<D1B9><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>
// ʹ<><CAB9><EFBFBD><EFBFBD>Clarke<6B>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD>-<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>ĵ<EFBFBD>ѹ<EFBFBD><D1B9>v<EFBFBD><76>, v<>£<EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>va, vb, vc<76><63><EFBFBD><EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>任<EFBFBD><E4BBBB>
Ua = Ualpha + voltage_power_supply / 2;
Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
Uc = (-Ualpha - sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѹӦ<D1B9><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѹӦ<D1B9><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PWM<57>ź<EFBFBD><C5BA>ϡ<EFBFBD>
// PWM<57><4D>ֵ<EFBFBD><D6B5>
setPWM(Ua, Ub, Uc);
}
// ///////////////////////////////////////////////////////////////////////////
// /***********************************************
// <20><><EFBFBD><EFBFBD>(Ratchet_Wheel)ģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Pֵ<50><D6B5><EFBFBD>ò<EFBFBD>ͬ<EFBFBD>ն<EFBFBD>(rigidity)
// DFOC_M0_setTorque(Kp*(target-DFOC_M0_Angle()));
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ã<EFBFBD>DFOC_M0_setTorque
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>Kp
// Ҫ<><D2AA><EFBFBD><EFBFBD>λ<EFBFBD>õ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>Ƕȣ<C7B6>target
// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD>Ƕȣ<C7B6>DFOC_M0_Angle
// ***********************************************/
// /*arduino dengfoc
// float attractor_distance = 45*PI/180.0;
// float target = round(DFOC_M0_Angle() / attractor_distance) * attractor_distance;
// DFOC_M0_setTorque(Kp * (target - DFOC_M0_Angle()));
// */
// void Ratchet_Wheel(float Angle)
// {
// // 8<><38><EFBFBD>ٴ<EFBFBD><D9B4><EFBFBD>,<2C>Ƕ<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD>ɻ<EFBFBD><C9BB><EFBFBD>ֵ<EFBFBD><D6B5>
// float attractor_distance = Angle * PI / 180.0f;
// // <20><><EFBFBD><EFBFBD>Pֵ,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӡ<EFBFBD>
// float P = 5;
// // <20><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>ֵ<EFBFBD><D6B5>
// float target = round(AS5600_GetAngle() / attractor_distance) * attractor_distance;
// // <20>նȿ<D5B6><C8BF>ơ<EFBFBD>
// setTorque(P * (target - AS5600_GetAngle()), _electricalAngle());
// }
// /***********************************************
// <20>߽<EFBFBD><DFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(Boundary_Moment_limitation)ģʽ<C4A3><CABD>
// 1<><31><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
// 2<><32><EFBFBD>趨<EFBFBD><E8B6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ť<EFBFBD><C5A5>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>Χ
// 3<><33><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// ***********************************************/
// void Boundary_Moment_limitation(float angle_range)
// {
// float P = 5;
// // <20>趨<EFBFBD><E8B6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ť<EFBFBD><C5A5>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>Χ 90<39><30>
// // float angle_range = 90.0;
// // <20>趨<EFBFBD><E8B6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ť<EFBFBD><C5A5>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>Χ<EFBFBD>м<EFBFBD>ֵ 45<34><35>
// float angle_range2 = angle_range / 2;
// // <20>м<EFBFBD>ֵת<D6B5><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// float limit_range = angle_range2 / 360.0f * _2PI;
// float L1 = zero_electric_angle + limit_range;
// float L2 = zero_electric_angle - limit_range;
// if (AS5600_GetAngle() > L2 && AS5600_GetAngle() < L1)
// {
// setTorque(0, _electricalAngle()); // <20>޶<EFBFBD><DEB6><EFBFBD>
// // 6<><36><EFBFBD>ٴ<EFBFBD><D9B4><EFBFBD>,<2C>Ƕ<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD>ɻ<EFBFBD><C9BB><EFBFBD>ֵ
// // float attractor_distance = 15 * PI / 180.0;
// // <20><><EFBFBD><EFBFBD>Pֵ
// // float P = 5;
// // float target = round(get_Angle()/attractor_distance)*attractor_distance;
// //<2F>նȿ<D5B6><C8BF><EFBFBD>
// setTorque(P*(target - get_Angle()),_electricalAngle());
// }
// else
// {
// if (AS5600_GetAngle() < L2)
// setTorque(P * (L2 - AS5600_GetAngle()), _electricalAngle());
// if (AS5600_GetAngle() > L1)
// setTorque(P * (L1 - AS5600_GetAngle()), _electricalAngle());
// }
// }
/***********************************************
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>(damping)ģʽ<EFBFBD><EFBFBD>
***********************************************/
/***********************************************
˳<EFBFBD><EFBFBD>(smooth)ģʽ<EFBFBD><EFBFBD>
***********************************************/
/***********************************************
ʧ<EFBFBD><EFBFBD>(weightlessness)ģʽ<EFBFBD><EFBFBD>
***********************************************/
Reference in New Issue Copy Permalink