240 lines
8.1 KiB
C
240 lines
8.1 KiB
C
////
|
|
//// Created by ZK on 2023/3/14.
|
|
////
|
|
////#include "main.h"
|
|
//#include <stdbool.h>
|
|
//#include "Controller.h"
|
|
//#include "SVPWM/SVPWM.h"
|
|
//#include "Communication.h"
|
|
//#include "Angle.h"
|
|
//
|
|
//#define UTILS_LP_FAST(value, sample, filter_constant) (value -= (filter_constant) * ((value) - (sample)))
|
|
//
|
|
//
|
|
//tFOC FOC;
|
|
//
|
|
//float uAlpha1;
|
|
//float uBeta1;
|
|
//
|
|
//#define PWM_FREQUENCY 24000
|
|
//#define CURRENT_MEASURE_HZ PWM_FREQUENCY
|
|
//#define CURRENT_MEASURE_PERIOD (float) (1.0f / (float) CURRENT_MEASURE_HZ)
|
|
//
|
|
//#define TIMER0_CLK_MHz 168
|
|
//#define PWM_PERIOD_CYCLES (uint16_t)((TIMER0_CLK_MHz * (uint32_t) 1000000u / ((uint32_t) (PWM_FREQUENCY))) & 0xFFFE)
|
|
//#define HALF_PWM_PERIOD_CYCLES (uint16_t)(PWM_PERIOD_CYCLES / 2U)
|
|
//float current1, current2, current3;
|
|
//
|
|
//float32_t id_curr_pi_kp = 0.001f;
|
|
//float32_t id_curr_pi_ki = 0.000001f;
|
|
//
|
|
//float32_t id_curr_pi_target = 0.0f;
|
|
//float32_t id_curr_pi_value;
|
|
//float32_t id_curr_pi_error;
|
|
//
|
|
//float32_t id_curr_pi_errMin = 0.0f;
|
|
//float32_t id_curr_pi_errSum;
|
|
//float32_t id_curr_pi_errSumMax = 30.0f;
|
|
//
|
|
//float32_t id_curr_pi_result;
|
|
//
|
|
//
|
|
//float32_t iq_curr_pi_kp = 0.001f;
|
|
//float32_t iq_curr_pi_ki = 0.000001f;
|
|
//
|
|
//float32_t iq_curr_pi_target = 1.0f;
|
|
//float32_t iq_curr_pi_value;
|
|
//float32_t iq_curr_pi_error;
|
|
//
|
|
//float32_t iq_curr_pi_errMin = 0.0f;
|
|
//float32_t iq_curr_pi_errSum;
|
|
//float32_t iq_curr_pi_errSumMax = 30.0f;
|
|
//
|
|
//float32_t iq_curr_pi_result;
|
|
//
|
|
//
|
|
//float32_t Speed_target = 0.0f;
|
|
//
|
|
//float32_t Speedpid_error;
|
|
//float32_t Speedpid_errSum;
|
|
//float32_t Speedpid_kp = 1.0f;
|
|
//float32_t Speedpid_ki = 0.1f;
|
|
//float32_t Speedpid_kd = 1.0f;
|
|
//float32_t Speedpid_errSumMax = 30.0f;
|
|
//float32_t Speedpid_lastErr;
|
|
//float32_t Speedpid_errDt;
|
|
//float32_t Speedpid_result;
|
|
//
|
|
//
|
|
//float32_t u_d, u_q;
|
|
//
|
|
//
|
|
//static void Current_PI_Cal_Id(float32_t resultMax) {
|
|
// //curr_pi_target = target;
|
|
//
|
|
// id_curr_pi_error = id_curr_pi_target - id_curr_pi_value;
|
|
//
|
|
//// if(curr_pi_error > PI_Control->errMin || curr_pi_error < -PI_Control->errMin)
|
|
// id_curr_pi_errSum += id_curr_pi_error * id_curr_pi_ki;
|
|
//
|
|
// if (id_curr_pi_errSum > id_curr_pi_errSumMax)
|
|
// id_curr_pi_errSum = id_curr_pi_errSumMax;
|
|
// else if (id_curr_pi_errSum < -id_curr_pi_errSumMax)
|
|
// id_curr_pi_errSum = -id_curr_pi_errSumMax;
|
|
//
|
|
// id_curr_pi_result = id_curr_pi_kp * id_curr_pi_error + id_curr_pi_errSum;
|
|
//
|
|
// if (id_curr_pi_result > resultMax)
|
|
// id_curr_pi_result = resultMax;
|
|
// else if (id_curr_pi_result < -resultMax)
|
|
// id_curr_pi_result = -resultMax;
|
|
//}
|
|
//
|
|
//
|
|
//static void Current_PI_Cal_Iq(float32_t resultMax) {
|
|
// //curr_pi_target = target;
|
|
//
|
|
// iq_curr_pi_error = iq_curr_pi_target - iq_curr_pi_value;
|
|
//
|
|
//// if(curr_pi_error > PI_Control->errMin || curr_pi_error < -PI_Control->errMin)
|
|
// iq_curr_pi_errSum += iq_curr_pi_error * iq_curr_pi_ki;
|
|
//
|
|
// if (iq_curr_pi_errSum > iq_curr_pi_errSumMax)
|
|
// iq_curr_pi_errSum = iq_curr_pi_errSumMax;
|
|
// else if (iq_curr_pi_errSum < -iq_curr_pi_errSumMax)
|
|
// iq_curr_pi_errSum = -iq_curr_pi_errSumMax;
|
|
//
|
|
// iq_curr_pi_result = iq_curr_pi_kp * iq_curr_pi_error + iq_curr_pi_errSum;
|
|
//
|
|
// if (iq_curr_pi_result > resultMax)
|
|
// iq_curr_pi_result = resultMax;
|
|
// else if (iq_curr_pi_result < -resultMax)
|
|
// iq_curr_pi_result = -resultMax;
|
|
//}
|
|
//
|
|
//
|
|
//static float32_t PIDGetResult(float32_t Speedpid_value, float32_t valMax, float32_t errMin) {
|
|
// Speedpid_error = Speed_target - Speedpid_value;
|
|
//
|
|
// if (Speedpid_error > errMin || Speedpid_error < -errMin)
|
|
// Speedpid_errSum += Speedpid_error * Speedpid_ki;
|
|
//
|
|
// if (Speedpid_errSum > Speedpid_errSumMax)
|
|
// Speedpid_errSum = Speedpid_errSumMax;
|
|
// else if (Speedpid_errSum < -Speedpid_errSumMax)
|
|
// Speedpid_errSum = -Speedpid_errSumMax;
|
|
//
|
|
// Speedpid_errDt = Speedpid_error - Speedpid_lastErr;
|
|
// Speedpid_lastErr = Speedpid_error;
|
|
//
|
|
// Speedpid_result = Speedpid_kp * Speedpid_error + Speedpid_errSum + Speedpid_kd * Speedpid_errDt;
|
|
//
|
|
// if (Speedpid_result > valMax)
|
|
// Speedpid_result = valMax;
|
|
// else if (Speedpid_result < -valMax)
|
|
// Speedpid_result = -valMax;
|
|
//
|
|
// return Speedpid_result;
|
|
//}
|
|
//
|
|
//
|
|
//void SpeedControl(float32_t target, float32_t angleVal, float32_t valMax) {
|
|
// Speed_target = target;
|
|
// float32_t motorControl_speedValue = GetSpeed(angleVal);
|
|
//// SendCurrent_Vofa(motorControl_speedValue, target, 0);
|
|
//// speedPID_value = motorControl_speedValue;
|
|
//
|
|
// iq_curr_pi_target = PIDGetResult(motorControl_speedValue, valMax, 0.0f);
|
|
//}
|
|
//
|
|
//
|
|
//
|
|
//
|
|
////bool Generate_SVM(float ud, float uq, float Theta) {
|
|
//bool FOC_current(float Id_set, float Iq_set, float Theta, float bw) {
|
|
// if (id_curr_pi_target > Id_set) { id_curr_pi_target = Id_set; }
|
|
// if (iq_curr_pi_target > Iq_set) { iq_curr_pi_target = Iq_set; }
|
|
//// id_curr_pi_target = Id_set;
|
|
//// iq_curr_pi_target = Iq_set;
|
|
//
|
|
// current1 = ADC1->JDR2;
|
|
// current2 = ADC2->JDR1;
|
|
// current3 = ADC1->JDR1;
|
|
// current1 = (current1 - 2048) * ((3.3f / 4095.0f) / 0.005f / 7.333333f) + 0.23;
|
|
// current2 = (current2 - 2048) * ((3.3f / 4095.0f) / 0.005f / 7.333333f);
|
|
// current3 = (current3 - 2048) * ((3.3f / 4095.0f) / 0.005f / 7.333333f) - 0.4;
|
|
//// SendCurrent_Vofa(current1, current2, current3);
|
|
//
|
|
// // Clarke transform
|
|
// float i_alpha, i_beta;
|
|
// clarke_transform(current1, current2, current3, &i_alpha, &i_beta);
|
|
// // Park transform
|
|
// float i_d, i_q;
|
|
// park_transform(i_alpha, i_beta, Theta, &i_d, &i_q);
|
|
//
|
|
// id_curr_pi_value = i_d;
|
|
// iq_curr_pi_value = i_q;
|
|
// Current_PI_Cal_Id(1.0f);
|
|
// Current_PI_Cal_Iq(1.0f);
|
|
// u_d = id_curr_pi_result;
|
|
// u_q = iq_curr_pi_result;
|
|
//
|
|
//
|
|
// // float mod_to_V = FOC.v_bus_filt * 2.0f / 3.0f;
|
|
// // float V_to_mod = 1.0f / mod_to_V;
|
|
// float mod_to_V = 1.2f * 2.0f / 3.0f;
|
|
// float V_to_mod = 1.0f / mod_to_V;
|
|
//
|
|
// float bandwidth = MIN(bw, 0.25f * PWM_FREQUENCY);
|
|
// // Apply PI control
|
|
// float Ierr_d = Id_set - i_d;
|
|
// float Ierr_q = Iq_set - i_q;
|
|
//// FOC.current_ctrl_p_gain = 5.0f * bandwidth;
|
|
//// FOC.current_ctrl_i_gain = 0.000002f * bandwidth;
|
|
// FOC.current_ctrl_p_gain = 0.001f;
|
|
// FOC.current_ctrl_i_gain = 0.00001f;
|
|
// FOC.current_ctrl_integral_d = 0;
|
|
// FOC.current_ctrl_integral_q = 0;
|
|
// float mod_d = V_to_mod * (FOC.current_ctrl_integral_d + Ierr_d * FOC.current_ctrl_p_gain);
|
|
// float mod_q = V_to_mod * (FOC.current_ctrl_integral_q + Ierr_q * FOC.current_ctrl_p_gain);
|
|
//
|
|
// // Vector modulation saturation, lock integrator if saturated
|
|
// float mod_scalefactor = 0.9f * SQRT3_BY_2 / sqrtf(SQ(mod_d) + SQ(mod_q));
|
|
// if (mod_scalefactor < 1.0f) {
|
|
// mod_d *= mod_scalefactor;
|
|
// mod_q *= mod_scalefactor;
|
|
// FOC.current_ctrl_integral_d *= 0.99f;
|
|
// FOC.current_ctrl_integral_q *= 0.99f;
|
|
// } else {
|
|
// FOC.current_ctrl_integral_d += Ierr_d * (FOC.current_ctrl_i_gain * CURRENT_MEASURE_PERIOD);
|
|
// FOC.current_ctrl_integral_q += Ierr_q * (FOC.current_ctrl_i_gain * CURRENT_MEASURE_PERIOD);
|
|
// }
|
|
//
|
|
//
|
|
// float pwm_phase = Theta + Theta * M_2PI * 20 * CURRENT_MEASURE_PERIOD;
|
|
// inversePark(u_d, u_q, Theta, &uAlpha1, &uBeta1);
|
|
//
|
|
// FOC.i_q = i_q;
|
|
// UTILS_LP_FAST(FOC.i_q_filt, FOC.i_q, 0.01f);
|
|
// FOC.i_d = i_d;
|
|
// UTILS_LP_FAST(FOC.i_d_filt, FOC.i_d, 0.01f);
|
|
// FOC.i_bus = (mod_d * i_d + mod_q * i_q);
|
|
// UTILS_LP_FAST(FOC.i_bus_filt, FOC.i_bus, 0.01f);
|
|
// FOC.power_filt = FOC.v_bus_filt * FOC.i_bus_filt;
|
|
//
|
|
// SVPWM(uAlpha1, uBeta1, &FOC.dtc_a, &FOC.dtc_b, &FOC.dtc_c);
|
|
//// FOC.dtc_a = FOC.dtc_a * 0.01;
|
|
//// FOC.dtc_b = FOC.dtc_b * 0.01;
|
|
//// FOC.dtc_c = FOC.dtc_c * 0.01;
|
|
// SendCurrent_Vofa(mod_d, mod_q, iq_curr_pi_target);
|
|
// TIM1->CCR1 = (uint16_t) (FOC.dtc_c * (float) HALF_PWM_PERIOD_CYCLES);
|
|
// TIM1->CCR2 = (uint16_t) (FOC.dtc_b * (float) HALF_PWM_PERIOD_CYCLES);
|
|
// TIM1->CCR3 = (uint16_t) (FOC.dtc_a * (float) HALF_PWM_PERIOD_CYCLES);
|
|
//// TIM1->CCR1 = (uint16_t) FOC.dtc_a;
|
|
//// TIM1->CCR2 = (uint16_t) FOC.dtc_b;
|
|
//// TIM1->CCR3 = (uint16_t) FOC.dtc_c;
|
|
//
|
|
// return 0;
|
|
//}
|
|
//
|