4x-tech/foc_ccs
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

test velocityopenloop failed

Browse Source
This commit is contained in:
4x-tech
2025-11-18 15:25:57 +08:00
parent a924ee9b94
commit 77b61cb90e
4 changed files with 115 additions and 143 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
3rd/config.h
View File

@@ -1,7 +1,7 @@
#ifndef CONFIG_H #ifndef CONFIG_H
#define CONFIG_H #define CONFIG_H
#define DEBUG_ENABLED true #define DEBUG_ENABLED false
#define DEBUG_MT_ENABLED true #define DEBUG_MT_ENABLED false
#define DEBUG_DFOC_ENABLED false #define DEBUG_DFOC_ENABLED false
#endif #endif

238
3rd/dfoc.c
View File

@@ -1,186 +1,156 @@
#include <stdlib.h> #include "config.h"
#include <stdio.h>
#include "pwm.h"
#include "mt6701.h"
#include "delay.h" #include "delay.h"
#include "lowpass_filter.h" #include "lowpass_filter.h"
#include "mt6701.h"
#include "pid_control.h" #include "pid_control.h"
#include "pwm.h"
#include <math.h> #include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <ti_msp_dl_config.h> #include <ti_msp_dl_config.h>
#include "config.h"
#define PI 3.14159265359f #define PI 3.14159265359f
#define _3PI_2 4.71238898f #define _3PI_2 4.71238898f
#define _1_SQRT3 0.57735026919f #define _1_SQRT3 0.57735026919f
#define _2_SQRT3 1.15470053838f #define _2_SQRT3 1.15470053838f
volatile float Ua = 0, Ub = 0, Uc = 0, Ualpha, Ubeta = 0, dc_a = 0, dc_b = 0, dc_c = 0; volatile float Ua = 0, Ub = 0, Uc = 0, Ualpha, Ubeta = 0, dc_a = 0, dc_b = 0,
const float voltage_limit = 8; dc_c = 0;
const float voltage_limit = 5.5;
const float voltage_power_supply = 12.0f; const float voltage_power_supply = 12.0f;
volatile float zero_electric_Angle = 0.0; volatile float zero_electric_Angle = 0.0;
extern int pp; extern int pp;
extern int Dir; extern int Dir;
void Motor_en() {
// GPIO_InitTypeDef GPIO_InitStructure;
void Motor_en() //
{ // RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
// GPIO_InitTypeDef GPIO_InitStructure; //
// // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
// RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE); // GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
// // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; // GPIO_Init(GPIOA,&GPIO_InitStructure);
// GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8; //
// GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; // GPIO_SetBits(GPIOA, GPIO_Pin_8);
// GPIO_Init(GPIOA,&GPIO_InitStructure);
//
// GPIO_SetBits(GPIOA, GPIO_Pin_8);
} }
//限制幅值 // 限制幅值
float constrain(float amt, float low, float high) float constrain(float amt, float low, float high) {
{ return ((amt < low) ? (low) : ((amt) > (high) ? (high) : (amt)));
return ((amt < low) ? (low) : ((amt) > (high) ? (high) : (amt)));
} }
//将角度归化到0-2PI // 将角度归化到0-2PI
float normalizeAngle(float angle) float normalizeAngle(float angle) {
{ volatile float a = fmod(angle, 2 * PI);
volatile float a = fmod(angle, 2 * PI); return ((a >= 0) ? a : (a + 2 * PI));
return ((a >= 0) ? a : (a + 2 * PI));
} }
float electricAngle(void) float electricAngle(void) {
{ return normalizeAngle((GetAngle_NoTrack() * pp * Dir) - zero_electric_Angle);
return normalizeAngle((GetAngle_NoTrack() * pp * Dir) - zero_electric_Angle);
} }
void SetPwm(float Ua, float Ub, float Uc) void SetPwm(float Ua, float Ub, float Uc) {
{ volatile float U_a = 0.0;
volatile float U_a = 0.0; volatile float U_b = 0.0;
volatile float U_b = 0.0; volatile float U_c = 0.0;
volatile float U_c = 0.0;
U_a = constrain(Ua, 0.0f, voltage_limit);
U_b = constrain(Ub, 0.0f, voltage_limit);
U_c = constrain(Uc, 0.0f, voltage_limit);
//printf("Ua : %f -- Ub : %f -- Uc : %f -- U_a : %f -- U_b : %f -- U_c : %f \n",Ua,Ub,Uc,U_a,U_b,U_c);
dc_a = constrain(U_a / voltage_power_supply, 0.0f, 1.0f);
dc_b = constrain(U_b / voltage_power_supply, 0.0f, 1.0f);
dc_c = constrain(U_c / voltage_power_supply, 0.0f, 1.0f);
PWM_Channel1(dc_a * 1000.0f); // 频率15k
PWM_Channel2(dc_b * 1000.0f);
PWM_Channel3(dc_c * 1000.0f);
U_a = constrain(Ua, 0.0f, voltage_limit);
U_b = constrain(Ub, 0.0f, voltage_limit);
U_c = constrain(Uc, 0.0f, voltage_limit);
// printf("Ua : %f -- Ub : %f -- Uc : %f -- U_a : %f -- U_b : %f -- U_c : %f
// \n",Ua,Ub,Uc,U_a,U_b,U_c);
dc_a = constrain(U_a / voltage_power_supply, 0.0f, 1.0f);
dc_b = constrain(U_b / voltage_power_supply, 0.0f, 1.0f);
dc_c = constrain(U_c / voltage_power_supply, 0.0f, 1.0f);
PWM_Channel1(dc_a * 500.0f); // 频率15k
PWM_Channel2(dc_b * 500.0f);
PWM_Channel3(dc_c * 500.0f);
} }
//FOC核心算法，克拉克逆变换/帕克逆变换 // FOC核心算法，克拉克逆变换/帕克逆变换
volatile float test_angle = 0.0; volatile float test_angle = 0.0;
volatile float last_test_angle = 0.0; volatile float last_test_angle = 0.0;
void SetPhaseVoltage(float Uq, float Ud, float angle_el) void SetPhaseVoltage(float Uq, float Ud, float angle_el) {
{ // angle_el = normalizeAngle(angle_el);
// angle_el = normalizeAngle(angle_el); test_angle = angle_el - last_test_angle;
test_angle = angle_el - last_test_angle;
Ualpha = -Uq * sin(angle_el); Ualpha = -Uq * sin(angle_el);
Ubeta = Uq * cos(angle_el); Ubeta = Uq * cos(angle_el);
Ua = Ualpha + voltage_power_supply / 2; Ua = Ualpha + voltage_power_supply / 2;
Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2; Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
Uc = -(Ualpha + sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2; Uc = -(Ualpha + sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2;
SetPwm(Ua, Ub, Uc); SetPwm(Ua, Ub, Uc);
last_test_angle = angle_el;
last_test_angle = angle_el;
} }
void Check_Sensor(void) void Check_Sensor(void) {
{ // SetPhaseVoltage(3, 0, _3PI_2);
//SetPhaseVoltage(3, 0, _3PI_2); // delay_ms(3000);
//delay_ms(3000); zero_electric_Angle = electricAngle();
zero_electric_Angle = electricAngle(); // SetPhaseVoltage(0, 0, _3PI_2);
// SetPhaseVoltage(0, 0, _3PI_2); // delay_ms(500);
//delay_ms(500);
} }
void FOC_Init(float power) void FOC_Init(float power) {
{ // voltage_power_supply = power;
//voltage_power_supply = power; // PWM_Init();
//PWM_Init(); // CurrSense_Init();
//CurrSense_Init(); // AS5600_Init();
//AS5600_Init();
Check_Sensor(); Check_Sensor();
} }
// 单角度环 // 单角度环
void Set_Angle(float Target) void Set_Angle(float Target) {
{ volatile float langle = GetAngle();
volatile float langle = GetAngle(); if (DEBUG_ENABLED & DEBUG_DFOC_ENABLED) {
if(DEBUG_ENABLED & DEBUG_DFOC_ENABLED) printf("angle readback in dfoc.c is %f \n", langle);
{ }
printf("angle readback in dfoc.c is %f \n", langle); volatile float Uq =
} PID_Controller(0.067, 0.01, 0, (Target - Dir * langle) * 180 / PI);
volatile float Uq = PID_Controller(0.067, 0.01, 0, (Target - Dir * langle) * 180 / PI); // volatile float Uq = PID_Controller(0.06, 0, 0, (Target - Dir * langle) *
//volatile float Uq = PID_Controller(0.06, 0, 0, (Target - Dir * langle) * 180 / PI); // 180 / PI);
if(DEBUG_ENABLED& DEBUG_DFOC_ENABLED) if (DEBUG_ENABLED & DEBUG_DFOC_ENABLED) {
{ printf("Uq is %f \n", Uq);
printf("Uq is %f \n", Uq); }
} SetPhaseVoltage(Uq, 0, electricAngle());
SetPhaseVoltage(Uq, 0, electricAngle());
} }
volatile double openloop_timestamp; volatile double openloop_timestamp;
float velocityopenloop(float target) float velocityopenloop(float target) {
{ volatile float Uq = 0.0;
volatile float Uq = 0.0; volatile double Ts = 0.0;
volatile double Ts = 0.0; volatile double shaft_angle;
volatile double shaft_angle;
volatile uint32_t now_ts = DL_SYSTICK_getValue(); volatile uint32_t now_ts = DL_SYSTICK_getValue();
if (now_ts < openloop_timestamp) {
Ts = (openloop_timestamp - now_ts) / 80.0f * 1e-6f;
} else {
Ts = (0xFFFFFF - now_ts + openloop_timestamp) / 80.0f * 1e-6f;
}
if(now_ts < openloop_timestamp) if (Ts < 0 || Ts >= 0.005) {
{ Ts = 0.001f;
Ts = (openloop_timestamp - now_ts) / 80.0f * 1e-6f; }
}
else
{
Ts = (0xFFFFFF - now_ts + openloop_timestamp) / 80.0f * 1e-6f;
}
if(Ts < 0 || Ts >= 0.005) shaft_angle = normalizeAngle(shaft_angle + pp * target * Ts);
{
Ts = 0.001f;
}
shaft_angle = normalizeAngle(shaft_angle + pp * target * Ts); if (DEBUG_ENABLED) {
printf("shaft_angle : %f -- Ts : %f \n", shaft_angle, Ts);
}
if(DEBUG_ENABLED) Uq = voltage_limit;
{
//printf("shaft_angle : %f -- target : %f -- Ts : %f \n",shaft_angle,target,Ts);
}
SetPhaseVoltage(Uq, 0, shaft_angle);
openloop_timestamp = now_ts;
Uq = voltage_limit; return Uq;
SetPhaseVoltage(Uq, 0, shaft_angle);
printf("shaft_angle : %f \n", shaft_angle);
openloop_timestamp = now_ts;
return Uq;
} }

15
empty.c
View File

@@ -48,7 +48,7 @@ extern bool gIsI2cError;
const float num_f = 0.123456f; const float num_f = 0.123456f;
volatile uint16_t count = 0; volatile uint16_t count = 0;
volatile float Target = 0; // 串口目标值 volatile float Target = 15; // 串口目标值
const int pp = 7; // 电机极对数 const int pp = 7; // 电机极对数
const int Dir = -1; // 电机编码器方向 const int Dir = -1; // 电机编码器方向
@@ -116,15 +116,18 @@ int main(void) {
// delay_ms(10); // delay_ms(10);
// 开环 // 开环
// velocityopenloop(Target); velocityopenloop(Target);
// 闭环
Set_Angle(Target); // //test MT6701
// MT6701_get_angle_degree();
// printf("angle degree is %f \n", angle_f);
// // 闭环
// Set_Angle(Target);
if (gCheckUART) { if (gCheckUART) {
gCheckUART = false; gCheckUART = false;
parse_uart_cmd(); parse_uart_cmd();
// Set_Angle(Target);
} }
delay_ms(10);
} }
} }

1
empty.syscfg
View File

@@ -74,7 +74,6 @@ I2C1.sclPinConfig.$name = "ti_driverlib_gpio_GPIOPinGeneric3";
PWM1.$name = "PWM_0"; PWM1.$name = "PWM_0";
PWM1.ccIndex = [0,1,2]; PWM1.ccIndex = [0,1,2];
PWM1.clockDivider = 2; PWM1.clockDivider = 2;
PWM1.timerCount = 2000;
PWM1.pwmMode = "CENTER_ALIGN"; PWM1.pwmMode = "CENTER_ALIGN";
PWM1.PWM_CHANNEL_0.$name = "ti_driverlib_pwm_PWMTimerCC0"; PWM1.PWM_CHANNEL_0.$name = "ti_driverlib_pwm_PWMTimerCC0";
PWM1.PWM_CHANNEL_1.$name = "ti_driverlib_pwm_PWMTimerCC1"; PWM1.PWM_CHANNEL_1.$name = "ti_driverlib_pwm_PWMTimerCC1";