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

add uart rx; 4 bytes cmd parse

Browse Source
This commit is contained in:
4x-tech
2025-11-17 10:13:18 +08:00
parent 5b49a0ee24
commit 6de9d53e1c
7 changed files with 237 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
3rd/config.h
View File

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

25
3rd/dfoc.c
View File

@@ -16,7 +16,7 @@
#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;
const float voltage_limit = 12;
const float voltage_limit = 8;
const float voltage_power_supply = 12.0f;
volatile float zero_electric_Angle = 0.0;
@@ -66,7 +66,7 @@ void SetPwm(float Ua, float Ub, float Uc)
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);
//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);
@@ -96,8 +96,8 @@ void SetPhaseVoltage(float Uq, float Ud, float angle_el)
SetPwm(Ua, Ub, Uc);
last_test_angle = angle_el;
}
@@ -124,8 +124,17 @@ void FOC_Init(float power)
// 单角度环
void Set_Angle(float Target)
{
volatile float angle = GetAngle();
volatile float Uq = PID_Controller(0.133, 0.01, 0, (Target - Dir * angle) * 180 / PI);
volatile float langle = GetAngle();
if(DEBUG_ENABLED & DEBUG_DFOC_ENABLED)
{
printf("angle readback in dfoc.c is %f \n", langle);
}
volatile float Uq = PID_Controller(0.133, 0.01, 0, (Target - Dir * langle) * 180 / PI);
//volatile float Uq = PID_Controller(0.06, 0, 0, (Target - Dir * langle) * 180 / PI);
if(DEBUG_ENABLED)
{
printf("Uq is %f \n", Uq);
}
SetPhaseVoltage(Uq, 0, electricAngle());
}
@@ -157,14 +166,14 @@ float velocityopenloop(float target)
if(DEBUG_ENABLED)
{
//printf("shaft_angle : %f -- target : %f -- Ts : %f",shaft_angle,target,Ts);
//printf("shaft_angle : %f -- target : %f -- Ts : %f \n",shaft_angle,target,Ts);
}
Uq = voltage_limit;
SetPhaseVoltage(Uq, 0, shaft_angle);
printf("shaft_angle : %f \n",shaft_angle);
printf("shaft_angle : %f \n", shaft_angle);
openloop_timestamp = now_ts;
return Uq;

39
3rd/mt6701.c
View File

@@ -10,6 +10,8 @@ volatile float angle_f_rad;
volatile bool gIsI2cError = false;
#define DEBUG_I2C false
/* Data sent to the Target */
uint8_t gTxPacket[I2C_TX_PACKET_SIZE] =
{
@@ -52,7 +54,7 @@ void MT6701_iic_read_angel(void)
*/
gDelayCycles = (5 * (gI2CclockConfig.divideRatio + 1)) *
(CPUCLK_FREQ / gClockSelFreq);
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c before writing -------\n");
@@ -75,7 +77,7 @@ void MT6701_iic_read_angel(void)
DL_I2C_startControllerTransfer(I2C_1_INST, I2C_TARGET_ADDRESS,
DL_I2C_CONTROLLER_DIRECTION_TX, I2C_TX_PACKET_SIZE);
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c writing done -------\n");
}
@@ -94,7 +96,7 @@ void MT6701_iic_read_angel(void)
{
gIsI2cError = true;
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c error ------------------------------------------\n");
}
@@ -111,7 +113,7 @@ void MT6701_iic_read_angel(void)
/* Add delay between transfers */
delay_cycles(1000);
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c before reading -------\n");
}
@@ -131,7 +133,7 @@ void MT6701_iic_read_angel(void)
gRxPacket[i] = DL_I2C_receiveControllerData(I2C_1_INST);
}
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c reading done -------\n");
}
@@ -140,29 +142,36 @@ void MT6701_iic_read_angel(void)
}
volatile float Last_ts = 0.0;
volatile float last_angle = 0.0;
// 单圈值
float GetAngle_NoTrack(void)
{
MT6701_iic_read_angel();
angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
angle_f_rad = (float)angle * _2PI / 16384;
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
{
printf("angle_rad read back is %f \n", angle_f_rad);
}
return angle_f_rad;
}
volatile float full_rotations = 0.0;
volatile float Last_Angle = 0.0;
volatile float Last_Angle_rad = 0.0;
//多圈值
float GetAngle(void)
{
volatile float D_Angle = 0.0;
volatile float Angle = GetAngle_NoTrack();
D_Angle = Angle - Last_Angle;
volatile float D_Angle_rad = 0.0;
volatile float Angle_rad = GetAngle_NoTrack();
D_Angle_rad = Angle_rad - Last_Angle_rad;
if(fabs(D_Angle) > (0.8f * 2 * PI))
if(fabs(D_Angle_rad) > (0.8f * 2 * PI))
{
full_rotations = full_rotations + ((D_Angle > 0) ? -1 : 1);
full_rotations = full_rotations + ((D_Angle_rad > 0) ? -1 : 1);
}
Last_Angle = Angle;
Last_Angle_rad = Angle_rad;
return (full_rotations * 2 * PI + Last_Angle);
return (full_rotations * 2 * PI + Last_Angle_rad);
}
volatile float Last_Vel_ts = 0.0;
@@ -173,11 +182,11 @@ float GetVelocity(void)
volatile float Vel_ts = SysTick -> VAL;
if(Vel_ts < Last_Vel_ts)
{
dt = (Last_Vel_ts - Vel_ts) / 9 * 1e-6f;
dt = (Last_Vel_ts - Vel_ts) / 80 * 1e-6f;
}
else
{
dt = (0xFFFFFF - Vel_ts + Last_Vel_ts) / 9 * 1e-6f;
dt = (0xFFFFFF - Vel_ts + Last_Vel_ts) / 80 * 1e-6f;
}
if(dt < 0.0001)