Boooom/BoooomFOC_STSPIN32G4_EVB
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

修正电流限幅

Browse Source
This commit is contained in:
BoomPC
2024-12-02 02:45:48 +08:00
parent 721796e105
commit 58e529da77
73 changed files with 1920 additions and 12247 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
BoooomCTL/Common/PID/PID.c
View File

@@ -1,27 +1,27 @@
////
//// Created by ZK on 2023/3/14.
////
//
// Created by ZK on 2023/3/14.
//#include "PID.h"
//
#include "PID.h"
float PID_Generate(PID *pid) {
pid->error = pid->target - pid->value;
if (pid->error > pid->errMin || pid->error < -pid->errMin)
pid->errSum += pid->error * pid->ki;
if (pid->errSum > pid->errSumMax)
pid->errSum = pid->errSumMax;
else if (pid->errSum < -pid->errSumMax)
pid->errSum = -pid->errSumMax;
pid->errDt = pid->error - pid->lastErr;
pid->lastErr = pid->error;
pid->result = pid->kp * pid->error + pid->errSum + pid->kd * pid->errDt;
if (pid->result > pid->valMax)
pid->result = pid->valMax;
else if (pid->result < -pid->valMax)
pid->result = -pid->valMax;
}
//float PID_Generate(PID *pid) {
// pid->error = pid->target - pid->value;
//
// if (pid->error > pid->errMin || pid->error < -pid->errMin)
// pid->errSum += pid->error * pid->ki;
//
// if (pid->errSum > pid->errSumMax)
// pid->errSum = pid->errSumMax;
// else if (pid->errSum < -pid->errSumMax)
// pid->errSum = -pid->errSumMax;
//
// pid->errDt = pid->error - pid->lastErr;
// pid->lastErr = pid->error;
//
// pid->result = pid->kp * pid->error + pid->errSum + pid->kd * pid->errDt;
//
// if (pid->result > pid->valMax)
// pid->result = pid->valMax;
// else if (pid->result < -pid->valMax)
// pid->result = -pid->valMax;
//}

60
BoooomCTL/Common/PID/PID.h
View File

@@ -1,32 +1,32 @@
////
//// Created by ZK on 2023/3/14.
////
//
// Created by ZK on 2023/3/14.
//#ifndef BOOOOMFOC_STSPIN32G4_EVB_PID_H
//#define BOOOOMFOC_STSPIN32G4_EVB_PID_H
//
#ifndef BOOOOMFOC_STSPIN32G4_EVB_PID_H
#define BOOOOMFOC_STSPIN32G4_EVB_PID_H
typedef struct pid {
float kp;//比例系数
float ki;//积分系数
float kd;//微分系数
float target;
float value;
float error;
float errSum;
float errSumMax;
float errMin;
float valMax;
float lastErr;
float errDt;
float result;
} PID;
float PID_Generate(PID *pid);
#endif //BOOOOMFOC_STSPIN32G4_EVB_PID_H
//typedef struct pid {
// float kp;//比例系数
// float ki;//积分系数
// float kd;//微分系数
//
// float target;
// float value;
// float error;
//
// float errSum;
// float errSumMax;
//
// float errMin;
// float valMax;
//
// float lastErr;
// float errDt;
//
// float result;
//} PID;
//
//float PID_Generate(PID *pid);
//
//
//#endif //BOOOOMFOC_STSPIN32G4_EVB_PID_H

70
BoooomCTL/Communication/Communication.c
View File

@@ -1,43 +1,43 @@
////
//// Created by ZK on 2023/3/14.
////
//
// Created by ZK on 2023/3/14.
////#include "Communication.h"
//#include "APP_Main.h"
//
//#define BYTE0(dwTemp) (*(char*)(&dwTemp))
//#define BYTE1(dwTemp) (*((char*)(&dwTemp) + 1))
//#define BYTE2(dwTemp) (*((char*)(&dwTemp) + 2))
//#define BYTE3(dwTemp) (*((char*)(&dwTemp) + 3))
//
//uint8_t USBCDC_SendBuffur[APP_TX_DATA_SIZE];
//
//void SendCurrent_Vofa(float a, float b, float c) {
// uint16_t USBCDC_SendBuffur_count = 0;
//
//#include "Communication.h"
#include "APP_Main.h"
#define BYTE0(dwTemp) (*(char*)(&dwTemp))
#define BYTE1(dwTemp) (*((char*)(&dwTemp) + 1))
#define BYTE2(dwTemp) (*((char*)(&dwTemp) + 2))
#define BYTE3(dwTemp) (*((char*)(&dwTemp) + 3))
uint8_t USBCDC_SendBuffur[APP_TX_DATA_SIZE];
void SendCurrent_Vofa(float a, float b, float c) {
uint16_t USBCDC_SendBuffur_count = 0;
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(a);
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(a);
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE2(a);
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE3(a);
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(b);
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(b);
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE2(b);
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE3(b);
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(c);
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(c);
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE2(c);
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE3(c);
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(a);
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(a);
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE2(a);
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE3(a);
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(b);
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(b);
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE2(b);
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE3(b);
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(c);
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(c);
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = 0x00;
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = 0x00;
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = 0x80;
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = 0x7f;
CDC_Transmit_FS(USBCDC_SendBuffur, USBCDC_SendBuffur_count);
}
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE2(c);
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE3(c);
//// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(a);
//// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(a);
//// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(b);
//// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(b);
//// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(c);
//// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(c);
//
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = 0x00;
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = 0x00;
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = 0x80;
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = 0x7f;
//
// CDC_Transmit_FS(USBCDC_SendBuffur, USBCDC_SendBuffur_count);
//}

20
BoooomCTL/Communication/Communication.h
View File

@@ -1,12 +1,12 @@
////
//// Created by ZK on 2023/3/14.
////
//
// Created by ZK on 2023/3/14.
//#ifndef BOOOOMFOC_STSPIN32G4_EVB_COMMUNICATION_H
//#define BOOOOMFOC_STSPIN32G4_EVB_COMMUNICATION_H
//
#ifndef BOOOOMFOC_STSPIN32G4_EVB_COMMUNICATION_H
#define BOOOOMFOC_STSPIN32G4_EVB_COMMUNICATION_H
void SendCurrent_Vofa(float a, float b, float c);
#endif //BOOOOMFOC_STSPIN32G4_EVB_COMMUNICATION_H
//
//void SendCurrent_Vofa(float a, float b, float c);
//
//
//#endif //BOOOOMFOC_STSPIN32G4_EVB_COMMUNICATION_H

474
BoooomCTL/Controller/Controller.c
View File

@@ -1,239 +1,239 @@
////
//// 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"
//
// Created by ZK on 2023/3/14.
//#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;
//}
//
//#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;
}

56
BoooomCTL/Controller/Controller.h
View File

@@ -1,30 +1,30 @@
////
//// Created by ZK on 2023/3/14.
////
//
// Created by ZK on 2023/3/14.
//#ifndef BOOOOMFOC_STSPIN32G4_EVB_CONTROLLER_H
//#define BOOOOMFOC_STSPIN32G4_EVB_CONTROLLER_H
//
#ifndef BOOOOMFOC_STSPIN32G4_EVB_CONTROLLER_H
#define BOOOOMFOC_STSPIN32G4_EVB_CONTROLLER_H
#include "arm_math.h"
typedef struct sFOC {
bool is_armed;
float v_bus, v_bus_filt, i_a, i_b, i_c;
float i_q, i_q_filt, i_d, i_d_filt, i_bus, i_bus_filt, power_filt;
float dtc_a, dtc_b, dtc_c;
float current_ctrl_p_gain, current_ctrl_i_gain;
float current_ctrl_integral_d, current_ctrl_integral_q;
} tFOC;
extern tFOC FOC;
extern void SpeedControl(float32_t target, float32_t angleVal, float32_t valMax);
extern bool FOC_current(float Id_set, float Iq_set, float Theta, float bw);
#endif //BOOOOMFOC_STSPIN32G4_EVB_CONTROLLER_H
//#include "arm_math.h"
//
//typedef struct sFOC {
// bool is_armed;
//
// float v_bus, v_bus_filt, i_a, i_b, i_c;
//
// float i_q, i_q_filt, i_d, i_d_filt, i_bus, i_bus_filt, power_filt;
// float dtc_a, dtc_b, dtc_c;
//
// float current_ctrl_p_gain, current_ctrl_i_gain;
// float current_ctrl_integral_d, current_ctrl_integral_q;
//} tFOC;
//
//extern tFOC FOC;
//
//
//extern void SpeedControl(float32_t target, float32_t angleVal, float32_t valMax);
//
//extern bool FOC_current(float Id_set, float Iq_set, float Theta, float bw);
//
//
//#endif //BOOOOMFOC_STSPIN32G4_EVB_CONTROLLER_H

22
BoooomCTL/Controller/PreDrive/PreDrive.c
View File

@@ -1,22 +0,0 @@
//
// Created by ZK on 2023/3/14.
//
#include "PreDrive.h"
uint8_t PreDrive_Init_Buffur[2] = {0x00, 0x00};
bool PreDrive_Init(void) {
while (HAL_I2C_Mem_Write(&hi2c3, 0x8e, 0x0c, I2C_MEMADD_SIZE_8BIT, PreDrive_Init_Buffur, 1, 1000) != HAL_OK) {}
PreDrive_Init_Buffur[0] = 0x00;
while (HAL_I2C_Mem_Write(&hi2c3, 0x8e, 0x0c, I2C_MEMADD_SIZE_8BIT, PreDrive_Init_Buffur, 1, 1000) != HAL_OK) {}
PreDrive_Init_Buffur[0] = 0xff;
while (HAL_I2C_Mem_Write(&hi2c3, 0x8e, 0x09, I2C_MEMADD_SIZE_8BIT, PreDrive_Init_Buffur, 1, 1000) != HAL_OK) {}
PreDrive_Init_Buffur[0] = 0x00;
while (HAL_I2C_Mem_Write(&hi2c3, 0x8e, 0x09, I2C_MEMADD_SIZE_8BIT, PreDrive_Init_Buffur, 1, 1000) != HAL_OK) {}
HAL_Delay(10);
return 0;
}

13
BoooomCTL/Controller/PreDrive/PreDrive.h
View File

@@ -1,13 +0,0 @@
//
// Created by ZK on 2023/3/14.
//
#ifndef BOOOOMFOC_STSPIN32G4_EVB_PREDRIVE_H
#define BOOOOMFOC_STSPIN32G4_EVB_PREDRIVE_H
#include "APP_Main.h"
bool PreDrive_Init(void);
#endif //BOOOOMFOC_STSPIN32G4_EVB_PREDRIVE_H

336
BoooomCTL/Controller/SVPWM/SVPWM.c
View File

@@ -1,170 +1,170 @@
////
//// Created by ZK on 2023/3/16.
////
//
// Created by ZK on 2023/3/16.
//#include "SVPWM.h"
//
#include "SVPWM.h"
#define SVM 1
extern uint16_t cycleNum;
#define SQRT3 1.732050808f
#define LIMIT (float32_t)(0.9f / SQRT3)
#define LIMIT_UDC 16.0f
#define TS 3300
#define SQRT3_MULT_TS (float32_t)((float32_t)TS * SQRT3)
uint8_t sectionMap[7] = {0, 2, 6, 1, 4, 3, 5};
float32_t uAlpha, uBeta;
float32_t ud, uq;
float channel1, channel2, channel3, udc = 12;
float uAlpha, uBeta;
float32_t iAlpha, iBeta;
float32_t id, iq;
inline void clarke_transform(float Ia, float Ib, float Ic, float *Ialpha, float *Ibeta) {
*Ialpha = Ia;
*Ibeta = (Ib - Ic) * ONE_BY_SQRT3;
}
inline void park_transform(float Ialpha, float Ibeta, float Theta, float *Id, float *Iq) {
float s = sinf(Theta / 57.29577951326093f);
float c = cosf(Theta / 57.29577951326093f);
*Id = Ialpha * c + Ibeta * s;
*Iq = -Ialpha * s + Ibeta * c;
}
inline void inversePark(float ud, float uq, float Theta, float *uAlpha, float *uBeta) {
float s = sinf(Theta / 57.29577951326093f);
float c = cosf(Theta / 57.29577951326093f);
*uAlpha = ud * c - uq * s;
*uBeta = ud * s + uq * c;
}
inline int SVPWM(float uAlpha, float uBeta, float *tA, float *tB, float *tC) {
int Sextant;
if (uBeta >= 0.0f) {
if (uAlpha >= 0.0f) {
//quadrant I
if (ONE_BY_SQRT3 * uBeta > uAlpha)
Sextant = 2; //sextant v2-v3
else
Sextant = 1; //sextant v1-v2
} else {
//quadrant II
if (-ONE_BY_SQRT3 * uBeta > uAlpha)
Sextant = 3; //sextant v3-v4
else
Sextant = 2; //sextant v2-v3
}
} else {
if (uAlpha >= 0.0f) {
//quadrant IV
if (-ONE_BY_SQRT3 * uBeta > uAlpha)
Sextant = 5; //sextant v5-v6
else
Sextant = 6; //sextant v6-v1
} else {
//quadrant III
if (ONE_BY_SQRT3 * uBeta > uAlpha)
Sextant = 4; //sextant v4-v5
else
Sextant = 5; //sextant v5-v6
}
}
switch (Sextant) {
// sextant v1-v2
case 1: {
// Vector on-times
float t1 = uAlpha - ONE_BY_SQRT3 * uBeta;
float t2 = TWO_BY_SQRT3 * uBeta;
// PWM timings
*tA = (1.0f - t1 - t2) * 0.5f;
*tB = *tA + t1;
*tC = *tB + t2;
}
break;
// sextant v2-v3
case 2: {
// Vector on-times
float t2 = uAlpha + ONE_BY_SQRT3 * uBeta;
float t3 = -uAlpha + ONE_BY_SQRT3 * uBeta;
// PWM timings
*tB = (1.0f - t2 - t3) * 0.5f;
*tA = *tB + t3;
*tC = *tA + t2;
}
break;
// sextant v3-v4
case 3: {
// Vector on-times
float t3 = TWO_BY_SQRT3 * uBeta;
float t4 = -uAlpha - ONE_BY_SQRT3 * uBeta;
// PWM timings
*tB = (1.0f - t3 - t4) * 0.5f;
*tC = *tB + t3;
*tA = *tC + t4;
}
break;
// sextant v4-v5
case 4: {
// Vector on-times
float t4 = -uAlpha + ONE_BY_SQRT3 * uBeta;
float t5 = -TWO_BY_SQRT3 * uBeta;
// PWM timings
*tC = (1.0f - t4 - t5) * 0.5f;
*tB = *tC + t5;
*tA = *tB + t4;
}
break;
// sextant v5-v6
case 5: {
// Vector on-times
float t5 = -uAlpha - ONE_BY_SQRT3 * uBeta;
float t6 = uAlpha - ONE_BY_SQRT3 * uBeta;
// PWM timings
*tC = (1.0f - t5 - t6) * 0.5f;
*tA = *tC + t5;
*tB = *tA + t6;
}
break;
// sextant v6-v1
case 6: {
// Vector on-times
float t6 = -TWO_BY_SQRT3 * uBeta;
float t1 = uAlpha + ONE_BY_SQRT3 * uBeta;
// PWM timings
*tA = (1.0f - t6 - t1) * 0.5f;
*tC = *tA + t1;
*tB = *tC + t6;
}
break;
}
// if any of the results becomes NaN, result_valid will evaluate to false
int result_valid = *tA >= 0.0f && *tA <= 1.0f && *tB >= 0.0f && *tB <= 1.0f && *tC >= 0.0f && *tC <= 1.0f;
return result_valid ? 0 : -1;
// TIM1->CCR1 = channelA;
// TIM1->CCR2 = channelB;
// TIM1->CCR3 = channelC;
}
//
//#define SVM 1
//
//extern uint16_t cycleNum;
//
//#define SQRT3 1.732050808f
//#define LIMIT (float32_t)(0.9f / SQRT3)
//#define LIMIT_UDC 16.0f
//#define TS 3300
//#define SQRT3_MULT_TS (float32_t)((float32_t)TS * SQRT3)
//
//uint8_t sectionMap[7] = {0, 2, 6, 1, 4, 3, 5};
//float32_t uAlpha, uBeta;
//float32_t ud, uq;
//float channel1, channel2, channel3, udc = 12;
//float uAlpha, uBeta;
//float32_t iAlpha, iBeta;
//float32_t id, iq;
//
//
//inline void clarke_transform(float Ia, float Ib, float Ic, float *Ialpha, float *Ibeta) {
// *Ialpha = Ia;
// *Ibeta = (Ib - Ic) * ONE_BY_SQRT3;
//}
//
//inline void park_transform(float Ialpha, float Ibeta, float Theta, float *Id, float *Iq) {
// float s = sinf(Theta / 57.29577951326093f);
// float c = cosf(Theta / 57.29577951326093f);
// *Id = Ialpha * c + Ibeta * s;
// *Iq = -Ialpha * s + Ibeta * c;
//}
//
//
//inline void inversePark(float ud, float uq, float Theta, float *uAlpha, float *uBeta) {
// float s = sinf(Theta / 57.29577951326093f);
// float c = cosf(Theta / 57.29577951326093f);
// *uAlpha = ud * c - uq * s;
// *uBeta = ud * s + uq * c;
//}
//
//
//inline int SVPWM(float uAlpha, float uBeta, float *tA, float *tB, float *tC) {
// int Sextant;
//
// if (uBeta >= 0.0f) {
// if (uAlpha >= 0.0f) {
// //quadrant I
// if (ONE_BY_SQRT3 * uBeta > uAlpha)
// Sextant = 2; //sextant v2-v3
// else
// Sextant = 1; //sextant v1-v2
//
// } else {
// //quadrant II
// if (-ONE_BY_SQRT3 * uBeta > uAlpha)
// Sextant = 3; //sextant v3-v4
// else
// Sextant = 2; //sextant v2-v3
// }
// } else {
// if (uAlpha >= 0.0f) {
// //quadrant IV
// if (-ONE_BY_SQRT3 * uBeta > uAlpha)
// Sextant = 5; //sextant v5-v6
// else
// Sextant = 6; //sextant v6-v1
// } else {
// //quadrant III
// if (ONE_BY_SQRT3 * uBeta > uAlpha)
// Sextant = 4; //sextant v4-v5
// else
// Sextant = 5; //sextant v5-v6
// }
// }
//
// switch (Sextant) {
// // sextant v1-v2
// case 1: {
// // Vector on-times
// float t1 = uAlpha - ONE_BY_SQRT3 * uBeta;
// float t2 = TWO_BY_SQRT3 * uBeta;
//
// // PWM timings
// *tA = (1.0f - t1 - t2) * 0.5f;
// *tB = *tA + t1;
// *tC = *tB + t2;
// }
// break;
//
// // sextant v2-v3
// case 2: {
// // Vector on-times
// float t2 = uAlpha + ONE_BY_SQRT3 * uBeta;
// float t3 = -uAlpha + ONE_BY_SQRT3 * uBeta;
//
// // PWM timings
// *tB = (1.0f - t2 - t3) * 0.5f;
// *tA = *tB + t3;
// *tC = *tA + t2;
// }
// break;
//
// // sextant v3-v4
// case 3: {
// // Vector on-times
// float t3 = TWO_BY_SQRT3 * uBeta;
// float t4 = -uAlpha - ONE_BY_SQRT3 * uBeta;
//
// // PWM timings
// *tB = (1.0f - t3 - t4) * 0.5f;
// *tC = *tB + t3;
// *tA = *tC + t4;
// }
// break;
//
// // sextant v4-v5
// case 4: {
// // Vector on-times
// float t4 = -uAlpha + ONE_BY_SQRT3 * uBeta;
// float t5 = -TWO_BY_SQRT3 * uBeta;
//
// // PWM timings
// *tC = (1.0f - t4 - t5) * 0.5f;
// *tB = *tC + t5;
// *tA = *tB + t4;
// }
// break;
//
// // sextant v5-v6
// case 5: {
// // Vector on-times
// float t5 = -uAlpha - ONE_BY_SQRT3 * uBeta;
// float t6 = uAlpha - ONE_BY_SQRT3 * uBeta;
//
// // PWM timings
// *tC = (1.0f - t5 - t6) * 0.5f;
// *tA = *tC + t5;
// *tB = *tA + t6;
// }
// break;
//
// // sextant v6-v1
// case 6: {
// // Vector on-times
// float t6 = -TWO_BY_SQRT3 * uBeta;
// float t1 = uAlpha + ONE_BY_SQRT3 * uBeta;
//
// // PWM timings
// *tA = (1.0f - t6 - t1) * 0.5f;
// *tC = *tA + t1;
// *tB = *tC + t6;
// }
// break;
// }
//
// // if any of the results becomes NaN, result_valid will evaluate to false
// int result_valid = *tA >= 0.0f && *tA <= 1.0f && *tB >= 0.0f && *tB <= 1.0f && *tC >= 0.0f && *tC <= 1.0f;
//
// return result_valid ? 0 : -1;
//
//// TIM1->CCR1 = channelA;
//// TIM1->CCR2 = channelB;
//// TIM1->CCR3 = channelC;
//}

66
BoooomCTL/Controller/SVPWM/SVPWM.h
View File

@@ -1,36 +1,36 @@
////
//// Created by ZK on 2023/3/16.
////
//
// Created by ZK on 2023/3/16.
//#ifndef BOOOOMFOC_STSPIN32G4_EVB_SVPWM_H
//#define BOOOOMFOC_STSPIN32G4_EVB_SVPWM_H
//
#ifndef BOOOOMFOC_STSPIN32G4_EVB_SVPWM_H
#define BOOOOMFOC_STSPIN32G4_EVB_SVPWM_H
#include "arm_math.h"
//#define M_PI (3.14159265358f)
#define M_2PI (6.28318530716f)
#define ONE_BY_SQRT3 (0.57735026919f)
#define TWO_BY_SQRT3 (2.0f * 0.57735026919f)
#define SQRT3_BY_2 (0.86602540378f)
#define SQ(x) ((x) * (x))
#define ABS(x) ((x) > 0 ? (x) : -(x))
#define MAX(x, y) (((x) > (y)) ? (x) : (y))
#define MIN(x, y) (((x) < (y)) ? (x) : (y))
#define CLAMP(x, lower, upper) (MIN(upper, MAX(x, lower)))
#define FLOAT_EQU(floatA, floatB) ((ABS((floatA) - (floatB))) < 0.000001f)
//extern inline void clarke_transform(float Ia, float Ib, float Ic, float *Ialpha, float *Ibeta);
//#include "arm_math.h"
//
//extern inline void park_transform(float Ialpha, float Ibeta, float Theta, float *Id, float *Iq);
extern void clarke_transform(float Ia, float Ib, float Ic, float *Ialpha, float *Ibeta);
extern void park_transform(float Ialpha, float Ibeta, float Theta, float *Id, float *Iq);
extern void inversePark(float ud, float uq, float Theta, float *uAlpha, float *uBeta);
extern int SVPWM(float uAlpha, float uBeta, float *tA, float *tB, float *tC);
#endif //BOOOOMFOC_STSPIN32G4_EVB_SVPWM_H
////#define M_PI (3.14159265358f)
//#define M_2PI (6.28318530716f)
//#define ONE_BY_SQRT3 (0.57735026919f)
//#define TWO_BY_SQRT3 (2.0f * 0.57735026919f)
//#define SQRT3_BY_2 (0.86602540378f)
//
//#define SQ(x) ((x) * (x))
//#define ABS(x) ((x) > 0 ? (x) : -(x))
//#define MAX(x, y) (((x) > (y)) ? (x) : (y))
//#define MIN(x, y) (((x) < (y)) ? (x) : (y))
//#define CLAMP(x, lower, upper) (MIN(upper, MAX(x, lower)))
//#define FLOAT_EQU(floatA, floatB) ((ABS((floatA) - (floatB))) < 0.000001f)
//
////extern inline void clarke_transform(float Ia, float Ib, float Ic, float *Ialpha, float *Ibeta);
////
////extern inline void park_transform(float Ialpha, float Ibeta, float Theta, float *Id, float *Iq);
//
//extern void clarke_transform(float Ia, float Ib, float Ic, float *Ialpha, float *Ibeta);
//
//extern void park_transform(float Ialpha, float Ibeta, float Theta, float *Id, float *Iq);
//
//extern void inversePark(float ud, float uq, float Theta, float *uAlpha, float *uBeta);
//
//extern int SVPWM(float uAlpha, float uBeta, float *tA, float *tB, float *tC);
//
//
//#endif //BOOOOMFOC_STSPIN32G4_EVB_SVPWM_H

169
BoooomCTL/Data/Angle/Angle.c
View File

@@ -1,169 +0,0 @@
//
// 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;
}

107
BoooomCTL/Data/Angle/Angle.h
View File

@@ -1,107 +0,0 @@
//
// Created by ZK on 2023/3/14.
//
#ifndef BOOOOMFOC_STSPIN32G4_EVB_ANGLE_H
#define BOOOOMFOC_STSPIN32G4_EVB_ANGLE_H
//#include "APP_Main.h"
#include "arm_math.h"
#include "main.h"
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include "adc.h"
#include "dma.h"
#include "i2c.h"
#include "opamp.h"
#include "spi.h"
#include "tim.h"
#include "usb_device.h"
#include "gpio.h"
typedef enum {
MT6816,
AS5600
} tEncoderModel;
typedef struct {
tEncoderModel EncoderModel;
float32_t (*getAngle)();
} tAngle;
typedef struct {
tAngle Angle;
float32_t (*Angle_Init)();
} tData;
uint16_t MT_ReadAngle(void);
float32_t GetSpeed(uint16_t angleVal);
float32_t getAngle_MT6816();
float32_t getAngle_AS5600();
bool Data_Init(tData *data);
typedef struct sMT6825 {
bool no_mag;
bool over_speed;
uint32_t angle;
uint8_t rx_err_count;
uint8_t check_err_count;
} tMT6825;
typedef struct sEncoder {
int raw;
int count_in_cpr;
int count_in_cpr_prev;
int64_t shadow_count;
// pll use
float pos_cpr_counts;
float vel_estimate_counts;
float pos;
float vel;
float phase;
float phase_vel;
float pll_kp;
float pll_ki;
float interpolation;
float snap_threshold;
} tEncoder;
struct ENCODER_Type {
SPI_HandleTypeDef *hspi;
GPIO_TypeDef *CS_Port;
uint16_t CS_Pin;
uint16_t sendBuf[2];
uint16_t recvBuf[2];
uint16_t offset;
uint16_t angleVal, lastAngle;
int32_t cycleNum;
float32_t angle_360;
float32_t eAngle_360;
// int32_t angleArr[32][2];
// uint8_t cnt;
float32_t speed;
float32_t filt;
float32_t speedFilt;
};
#endif //BOOOOMFOC_STSPIN32G4_EVB_ANGLE_H

5
BoooomCTL/Data/Angle/MT6816/MT6816.c
View File

@@ -1,5 +0,0 @@
//
// Created by ZK on 2023/3/14.
//
#include "MT6816.h"

8
BoooomCTL/Data/Angle/MT6816/MT6816.h
View File

@@ -1,8 +0,0 @@
//
// Created by ZK on 2023/3/14.
//
#ifndef BOOOOMFOC_STSPIN32G4_EVB_MT6816_H
#define BOOOOMFOC_STSPIN32G4_EVB_MT6816_H
#endif //BOOOOMFOC_STSPIN32G4_EVB_MT6816_H

5
BoooomCTL/Data/Current/Current.c
View File

@@ -1,5 +0,0 @@
//
// Created by ZK on 2023/3/14.
//
#include "Current.h"

8
BoooomCTL/Data/Current/Current.h
View File

@@ -1,8 +0,0 @@
//
// Created by ZK on 2023/3/14.
//
#ifndef BOOOOMFOC_STSPIN32G4_EVB_CURRENT_H
#define BOOOOMFOC_STSPIN32G4_EVB_CURRENT_H
#endif //BOOOOMFOC_STSPIN32G4_EVB_CURRENT_H

22
BoooomCTL/Data/Current/InteriorADC/InteriorADC.c
View File

@@ -1,22 +0,0 @@
//
// Created by ZK on 2023/3/14.
//
#include "InteriorADC.h"
bool InteriorADC_Init(void) {
//使能ADC注入
HAL_ADCEx_Calibration_Start(&hadc1, ADC_SINGLE_ENDED);
HAL_ADCEx_Calibration_Start(&hadc2, ADC_SINGLE_ENDED);
HAL_Delay(100);
HAL_ADCEx_InjectedStart(&hadc1);
__HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC);
__HAL_ADC_DISABLE_IT(&hadc1, ADC_IT_JEOS);//关闭ADC1的中断，避免ADC1_2_IRQHandler触发两次
HAL_ADCEx_InjectedStart(&hadc2);
__HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC);
return 0;//返回值为零表示初始化成功
}

12
BoooomCTL/Data/Current/InteriorADC/InteriorADC.h
View File

@@ -1,12 +0,0 @@
//
// Created by ZK on 2023/3/14.
//
#ifndef BOOOOMFOC_STSPIN32G4_EVB_INTERIORADC_H
#define BOOOOMFOC_STSPIN32G4_EVB_INTERIORADC_H
#include "APP_Main.h"
bool InteriorADC_Init(void);
#endif //BOOOOMFOC_STSPIN32G4_EVB_INTERIORADC_H

5
BoooomCTL/Data/Data.c
View File

@@ -1,5 +0,0 @@
//
// Created by ZK on 2023/3/14.
//
#include "Data.h"

8
BoooomCTL/Data/Data.h
View File

@@ -1,8 +0,0 @@
//
// Created by ZK on 2023/3/14.
//
#ifndef BOOOOMFOC_STSPIN32G4_EVB_DATA_H
#define BOOOOMFOC_STSPIN32G4_EVB_DATA_H
#endif //BOOOOMFOC_STSPIN32G4_EVB_DATA_H

5
BoooomCTL/Data/Temperature/Temperature.c
View File

@@ -1,5 +0,0 @@
//
// Created by ZK on 2023/3/14.
//
#include "Temperature.h"

8
BoooomCTL/Data/Temperature/Temperature.h
View File

@@ -1,8 +0,0 @@
//
// Created by ZK on 2023/3/14.
//
#ifndef BOOOOMFOC_STSPIN32G4_EVB_TEMPERATURE_H
#define BOOOOMFOC_STSPIN32G4_EVB_TEMPERATURE_H
#endif //BOOOOMFOC_STSPIN32G4_EVB_TEMPERATURE_H

5
BoooomCTL/Data/Voltage/Voltage.c
View File

@@ -1,5 +0,0 @@
//
// Created by ZK on 2023/3/14.
//
#include "Voltage.h"

8
BoooomCTL/Data/Voltage/Voltage.h
View File

@@ -1,8 +0,0 @@
//
// Created by ZK on 2023/3/14.
//
#ifndef BOOOOMFOC_STSPIN32G4_EVB_VOLTAGE_H
#define BOOOOMFOC_STSPIN32G4_EVB_VOLTAGE_H
#endif //BOOOOMFOC_STSPIN32G4_EVB_VOLTAGE_H

5
BoooomCTL/Parameter/Parameter.c
View File

@@ -1,5 +0,0 @@
//
// Created by ZK on 2023/3/14.
//
#include "Parameter.h"

8
BoooomCTL/Parameter/Parameter.h
View File

@@ -1,8 +0,0 @@
//
// Created by ZK on 2023/3/14.
//
#ifndef BOOOOMFOC_STSPIN32G4_EVB_PARAMETER_H
#define BOOOOMFOC_STSPIN32G4_EVB_PARAMETER_H
#endif //BOOOOMFOC_STSPIN32G4_EVB_PARAMETER_H