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

Compare commits

base: 4x-tech:feature_vp_closed_loop
Branches Tags
4x-tech:master 4x-tech:feature_velocity_closed_loop 4x-tech:feature_vp_closed_loop
...
compare: 4x-tech:a2d2773ace4d7d20546d34d5056a18d19bbcda6a
Branches Tags
4x-tech:feature_velocity_closed_loop 4x-tech:feature_vp_closed_loop 4x-tech:master

3 Commits
feature_vp ... a2d2773ace

Author SHA1 Message Date
4x-tech
a2d2773ace correct SYSTICK and RELOAD_CYCLES 2025-11-20 16:32:17 +08:00
4x-tech
a9aa2e19c8 velocity-close-loop test passed;find the uart bug that uart cannot receive over 6 bytes 2025-11-20 15:05:28 +08:00
4x-tech
ed09f10c03 chenged to 2 motor velocity-closed-loop; before testing 2025-11-20 09:50:13 +08:00
14 changed files with 293 additions and 374 deletions
Expand all files Collapse all files

10
3rd/config.h
View File

@@ -1,7 +1,13 @@
#ifndef CONFIG_H #ifndef __CONFIG_H
#define CONFIG_H #define __CONFIG_H
// debug switch
#define DEBUG_ENABLED false #define DEBUG_ENABLED false
#define DEBUG_MT_ENABLED false #define DEBUG_MT_ENABLED false
#define DEBUG_DFOC_ENABLED false #define DEBUG_DFOC_ENABLED false
// systick
#define RELOAD_CYCLES 0xF423FF
#define MCLK_IN_MHZ 80
#endif #endif

159
3rd/dfoc.c
View File

@@ -1,4 +1,4 @@
#include "dfoc.h"
#include "config.h" #include "config.h"
#include "delay.h" #include "delay.h"
#include "lowpass_filter.h" #include "lowpass_filter.h"
@@ -15,6 +15,18 @@
#define _1_SQRT3 0.57735026919f #define _1_SQRT3 0.57735026919f
#define _2_SQRT3 1.15470053838f #define _2_SQRT3 1.15470053838f
struct Motor_ M0 = {0};
struct Motor_ M1 = {1};
struct _PID M0_VEL_PID = {0.4, 2, 0};
struct _PID M1_VEL_PID = {0.4, 2, 0};
struct LOWPASS M0_VEL_Filter = {0.1};
struct LOWPASS M1_VEL_Filter = {0.1};
struct MT6701 Angle_Sensor0 = {0};
struct MT6701 Angle_Sensor1 = {1};
#define VOLTAGE_POWER_SUPPLY 12.0f #define VOLTAGE_POWER_SUPPLY 12.0f
#define VOLTAGE_LIMIT 6 #define VOLTAGE_LIMIT 6
#define PWM_COMPARE_VALUE 750.0f #define PWM_COMPARE_VALUE 750.0f
@@ -22,10 +34,10 @@
volatile float Ua = 0, Ub = 0, Uc = 0, Ualpha, Ubeta = 0, dc_a = 0, dc_b = 0, volatile float Ua = 0, Ub = 0, Uc = 0, Ualpha, Ubeta = 0, dc_a = 0, dc_b = 0,
dc_c = 0; dc_c = 0;
volatile float zero_electric_Angle = 0.0; float M0_zero_elc_Angle = 0, M1_zero_elc_Angle = 0;
extern int pp; extern int M0_PP, M0_DIR;
extern int Dir; extern int M1_PP, M1_DIR;
void Motor_en() {} void Motor_en() {}
@@ -40,11 +52,16 @@ float normalizeAngle(float angle) {
return ((a >= 0) ? a : (a + 2 * PI)); return ((a >= 0) ? a : (a + 2 * PI));
} }
float electricAngle(void) { float M0_electricAngle(void) {
return normalizeAngle((GetAngle_NoTrack() * pp * Dir) - zero_electric_Angle); return normalizeAngle((GetAngle_NoTrack(&Angle_Sensor0) * M0_PP * M0_DIR) -
M0_zero_elc_Angle);
} }
void SetPwm(float Ua, float Ub, float Uc) { float M1_electricAngle(void) {
return normalizeAngle((GetAngle_NoTrack(&Angle_Sensor1) * M1_PP * M1_DIR) -
M1_zero_elc_Angle);
}
void SetPwm(int Mot_num, 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;
@@ -53,97 +70,87 @@ void SetPwm(float Ua, float Ub, float Uc) {
U_b = constrain(Ub, 0.0f, VOLTAGE_LIMIT); U_b = constrain(Ub, 0.0f, VOLTAGE_LIMIT);
U_c = constrain(Uc, 0.0f, VOLTAGE_LIMIT); U_c = constrain(Uc, 0.0f, VOLTAGE_LIMIT);
dc_a = constrain(U_a / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f); if (Mot_num == 0) {
dc_b = constrain(U_b / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f); M0.dc_a = constrain(U_a / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
dc_c = constrain(U_c / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f); M0.dc_b = constrain(U_b / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
if (DEBUG_ENABLED & DEBUG_DFOC_ENABLED) { M0.dc_c = constrain(U_c / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
printf("dc_a : %f -- dc_b : %f -- dc_c : %f \n", dc_a, dc_b, dc_c); M0_PWM_A(M0.dc_a * PWM_COMPARE_VALUE); // 频率15k
M0_PWM_B(M0.dc_b * PWM_COMPARE_VALUE);
M0_PWM_C(M0.dc_c * PWM_COMPARE_VALUE);
} else if (Mot_num == 1) {
M1.dc_a = constrain(U_a / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
M1.dc_b = constrain(U_b / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
M1.dc_c = constrain(U_c / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
M1_PWM_A(M1.dc_a * PWM_COMPARE_VALUE); // 频率15k
M1_PWM_B(M1.dc_b * PWM_COMPARE_VALUE);
M1_PWM_C(M1.dc_c * PWM_COMPARE_VALUE);
} }
PWM_Channel1((1 - dc_a) * PWM_COMPARE_VALUE); // 频率15k
PWM_Channel2((1 - dc_b) * PWM_COMPARE_VALUE);
PWM_Channel3((1 - dc_c) * PWM_COMPARE_VALUE);
} }
// FOC核心算法，克拉克逆变换/帕克逆变换 // FOC核心算法，克拉克逆变换/帕克逆变换
volatile float test_angle = 0.0;
volatile float last_test_angle = 0.0; void SetPhaseVoltage(struct Motor_ *Motor, float Uq, 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;
Ualpha = -Uq * sin(angle_el); Motor->Ualpha = -Uq * sin(angle_el);
Ubeta = Uq * cos(angle_el); Motor->Ubeta = Uq * cos(angle_el);
Ua = Ualpha + VOLTAGE_POWER_SUPPLY / 2; Motor->Ua = Motor->Ualpha + VOLTAGE_POWER_SUPPLY / 2;
Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + VOLTAGE_POWER_SUPPLY / 2; Motor->Ub =
Uc = -(Ualpha + sqrt(3) * Ubeta) / 2 + VOLTAGE_POWER_SUPPLY / 2; (sqrt(3) * Motor->Ubeta - Motor->Ualpha) / 2 + VOLTAGE_POWER_SUPPLY / 2;
Motor->Uc =
-(Motor->Ualpha + sqrt(3) * Motor->Ubeta) / 2 + VOLTAGE_POWER_SUPPLY / 2;
SetPwm(Ua, Ub, Uc); SetPwm(Motor->Mot_num, Motor->Ua, Motor->Ub, Motor->Uc);
last_test_angle = angle_el;
} }
void Check_Sensor(void) { void Check_Sensor(void) {
// SetPhaseVoltage(3, 0, _3PI_2); SetPhaseVoltage(&M0, 3, _3PI_2);
// delay_ms(3000); delay_ms(2000);
zero_electric_Angle = electricAngle(); M0_zero_elc_Angle = M0_electricAngle();
printf("Check_Sensor zero_electric_Angle is %f \n", zero_electric_Angle); SetPhaseVoltage(&M0, 0, _3PI_2);
// SetPhaseVoltage(0, 0, _3PI_2); delay_ms(500);
// delay_ms(500);
SetPhaseVoltage(&M1, 3, _3PI_2);
delay_ms(2000);
M1_zero_elc_Angle = M1_electricAngle();
SetPhaseVoltage(&M1, 0, _3PI_2);
delay_ms(500);
} }
void FOC_Init() { void FOC_Init() {
// PWM_Init(); // PWM_Init();
// CurrSense_Init(); // CurrSense_Init();
// AS5600_Init(); // mt6701_Init();
Check_Sensor(); Check_Sensor();
} }
// 单角度环 void Print_Velocity(int Motor_Velocity) {
void Set_Angle(float Target) { if (Motor_Velocity == 0) {
volatile float langle = GetAngle(); printf("M0:%f\n", GetVelocity(&Angle_Sensor0));
if (DEBUG_ENABLED & DEBUG_DFOC_ENABLED) {
printf("angle readback in dfoc.c is %f \n", langle);
} }
volatile float Uq = if (Motor_Velocity == 1) {
PID_Controller(0.03, 0.01, 0, (Target - Dir * langle) * 180 / PI); printf("M1:%f\n", GetVelocity(&Angle_Sensor1));
// volatile float Uq = PID_Controller(0.133, 0, 0, (Target - Dir * langle) *
// 180 / PI);
if (DEBUG_ENABLED & DEBUG_DFOC_ENABLED) {
printf("Uq is %f \n", Uq);
} }
SetPhaseVoltage(Uq, 0, electricAngle());
} }
volatile double openloop_timestamp; // 单速度环
float velocityopenloop(float target) { void M0_Set_Velocity(float Target) {
volatile float Uq = 0.0; Angle_Sensor0.velocity =
volatile double Ts = 0.0; Lowpassfilter(&M0_VEL_Filter, GetVelocity(&Angle_Sensor0));
volatile double shaft_angle; SetPhaseVoltage(
&M0,
volatile uint32_t now_ts = DL_SYSTICK_getValue(); PID_Controller(&M0_VEL_PID, M0_DIR * (Target - Angle_Sensor0.velocity)),
M0_electricAngle());
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 (Ts < 0 || Ts >= 0.005) {
Ts = 0.001f;
}
shaft_angle = normalizeAngle(shaft_angle + pp * target * Ts);
if (DEBUG_ENABLED) {
printf("shaft_angle : %f -- Ts : %f \n", shaft_angle, Ts);
}
Uq = VOLTAGE_LIMIT;
SetPhaseVoltage(Uq, 0, shaft_angle);
openloop_timestamp = now_ts;
return Uq;
} }
void M1_Set_Velocity(float Target) {
Angle_Sensor1.velocity =
Lowpassfilter(&M1_VEL_Filter, GetVelocity(&Angle_Sensor1));
SetPhaseVoltage(
&M1,
PID_Controller(&M1_VEL_PID, M1_DIR * (Target - Angle_Sensor1.velocity)),
M1_electricAngle());
}

35
3rd/dfoc.h
View File

@@ -1,20 +1,35 @@
#ifndef __DFOC_H #ifndef __DFOC_H
#define __DFOC_H #define __DFOC_H
struct Motor_ {
int Mot_num;
float Ua;
float Ub;
float Uc;
float Ubeta;
float Ualpha;
float dc_a;
float dc_b;
float dc_c;
};
void Motor_en(void); void Motor_en(void);
float constrain(float amt, float low, float high); float constrain(float amt, float low, float high);
void SetPwm(float Ua, float Ub, float Uc); void SetPwm(int Mot_num, float Ua, float Ub, float Uc);
float normalizeAngle(float angle); float normalizeAngle(float angle);
void SetPhaseVoltage(float Uq, float Ud, float angle_el); void SetPhaseVoltage(struct Motor_ *Motor, float Uq, float angle_el);
float cal_Iq_Id(float current_a, float current_b, float angle_el);
void Check_Sensor(void); void Check_Sensor(void);
void FOC_Init(float power); void FOC_Init(void);
float electricAngle(void); float M0_electricAngle(void);
float GetCommand(void); float M1_electricAngle(void);
void Set_Angle(float Target); void M0_Set_Angle(float Target);
void M0_Set_Velocity(float Target);
void M0_Set_CurTorque(float Target);
void M1_Set_Angle(float Target);
void M1_Set_Velocity(float Target);
void M1_Set_CurTorque(float Target);
float velocityopenloop(float target); float velocityopenloop(float target);
void Print_Velocity(int Motor_Velocity);
#endif #endif

64
3rd/lowpass_filter.c
View File

@@ -1,48 +1,40 @@
#include "lowpass_filter.h"
#include "config.h"
#include <stdint.h> #include <stdint.h>
#include <ti_msp_dl_config.h> #include <ti_msp_dl_config.h>
#define _2PI 6.28318530718f
float y = 0; float y = 0;
float Lowpassfilter_sim(float x) float Lowpassfilter_sim(float x) {
{ float out = 0.9 * x + 0.1 * y;
float out = 0.9 * x + 0.1 * y; y = x;
y = x; return out;
return out;
} }
uint32_t Last_Timesamp = 0.0; float Lowpassfilter(struct LOWPASS *lowpass, float x) {
float Last_y = 0.0; float dt = 0.0;
float Lowpassfilter(float Tf, float x)
{
float dt = 0.0;
uint32_t Timesamp = DL_SYSTICK_getValue(); uint32_t Timesamp = DL_SYSTICK_getValue();
if(Timesamp < Last_Timesamp) if (Timesamp < lowpass->Last_Timesamp)
{ dt = (float)(lowpass->Last_Timesamp - Timesamp) / MCLK_IN_MHZ * 1e-6;
dt = (float)(Last_Timesamp - Timesamp) / 80 * 1e-6; else
} dt = (float)(RELOAD_CYCLES - Timesamp + lowpass->Last_Timesamp) /
else MCLK_IN_MHZ * 1e-6;
{
dt = (float)(0xFFFFFF - Timesamp + Last_Timesamp) / 80 * 1e-6;
}
if(dt < 0.0 || dt == 0) if (dt < 0.0 || dt == 0)
{ dt = 0.0015f;
dt = 0.0015f; else if (dt > 0.005f) {
} lowpass->Last_y = x;
else if(dt > 0.005f) lowpass->Last_Timesamp = Timesamp;
{ return x;
Last_y = x; }
Last_Timesamp = Timesamp; float alpha = lowpass->Tf / (lowpass->Tf + dt);
return x; float y = alpha * lowpass->Last_y + (1.0f - alpha) * x;
}
float alpha = Tf / (Tf + dt);
float y = alpha * Last_y + (1.0f - alpha) * x;
Last_y = y; lowpass->Last_y = y;
Last_Timesamp = Timesamp; lowpass->Last_Timesamp = Timesamp;
return y;
return y;
} }

11
3rd/lowpass_filter.h
View File

@@ -1,12 +1,13 @@
#ifndef __LOWPASS_FILTER_H #ifndef __LOWPASS_FILTER_H
#define __LOWPASS_FILTER_H #define __LOWPASS_FILTER_H
struct LOWPASS {
float Tf;
float Last_Timesamp;
float Last_y;
};
float Lowpassfilter(float Tf, float x);
float Lowpassfilter_sim(float x); float Lowpassfilter_sim(float x);
float Lowpassfilter(struct LOWPASS *lowpass, float x);
#endif #endif

170
3rd/mt6701.c
View File

@@ -52,115 +52,9 @@ void MT6701_iic_read_angel(void) {
I2C_Stop(); I2C_Stop();
} }
// void MT6701_iic_read_angel(void) {
// /* Get I2C clock source and clock divider to use for delay cycle
// calculation */
// DL_I2C_getClockConfig(I2C_1_INST, &gI2CclockConfig);
// switch (gI2CclockConfig.clockSel) {
// case DL_I2C_CLOCK_BUSCLK:
// gClockSelFreq = 32000000;
// break;
// case DL_I2C_CLOCK_MFCLK:
// gClockSelFreq = 4000000;
// break;
// default:
// break;
// }
// /*
// * Calculate number of clock cycles to delay after controller transfer
// initiated
// * gDelayCycles = 3 I2C functional clock cycles
// * gDelayCycles = 3 * I2C clock divider * (CPU clock freq / I2C clock
// freq)
// */
// gDelayCycles =
// (5 * (gI2CclockConfig.divideRatio + 1)) * (CPUCLK_FREQ /
// gClockSelFreq);
// if (DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C) {
// printf("i2c before writing -------\n");
// }
// /*
// * Fill FIFO with data. This example will send a MAX of 8 bytes since it
// * doesn't handle the case where FIFO is full
// */
// DL_I2C_fillControllerTXFIFO(I2C_1_INST, &gTxPacket[0], I2C_TX_PACKET_SIZE);
// /* Wait for I2C to be Idle */
// while (
// !(DL_I2C_getControllerStatus(I2C_1_INST) &
// DL_I2C_CONTROLLER_STATUS_IDLE))
// ;
// /* Send the packet to the controller.
// * This function will send Start + Stop automatically.
// */
// DL_I2C_startControllerTransfer(I2C_1_INST, I2C_TARGET_ADDRESS,
// DL_I2C_CONTROLLER_DIRECTION_TX,
// I2C_TX_PACKET_SIZE);
// if (DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C) {
// printf("i2c writing done -------\n");
// }
// /* Workaround for errata I2C_ERR_13 */
// delay_cycles(gDelayCycles);
// /* Poll until the Controller writes all bytes */
// while (DL_I2C_getControllerStatus(I2C_1_INST) &
// DL_I2C_CONTROLLER_STATUS_BUSY)
// ;
// /* Trap if there was an error */
// if (DL_I2C_getControllerStatus(I2C_1_INST) &
// DL_I2C_CONTROLLER_STATUS_ERROR) {
// gIsI2cError = true;
// if (DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C) {
// printf("i2c error ------------------------------------------\n");
// }
// /* LED will remain high if there is an error */
// __BKPT(0);
// return;
// }
// /* Wait for I2C to be Idle */
// while (
// !(DL_I2C_getControllerStatus(I2C_1_INST) &
// DL_I2C_CONTROLLER_STATUS_IDLE))
// ;
// /* Add delay between transfers */
// delay_cycles(1000);
// if (DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C) {
// printf("i2c before reading -------\n");
// }
// /* Send a read request to Target */
// DL_I2C_startControllerTransfer(I2C_1_INST, I2C_TARGET_ADDRESS,
// DL_I2C_CONTROLLER_DIRECTION_RX,
// I2C_RX_PACKET_SIZE);
// /*
// * Receive all bytes from target. LED will remain high if not all bytes
// * are received
// */
// for (uint8_t i = 0; i < I2C_RX_PACKET_SIZE; i++) {
// while (DL_I2C_isControllerRXFIFOEmpty(I2C_1_INST))
// ;
// gRxPacket[i] = DL_I2C_receiveControllerData(I2C_1_INST);
// }
// if (DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C) {
// printf("i2c reading done -------\n");
// }
// }
volatile float Last_ts = 0.0;
volatile float last_angle = 0.0;
// 单圈值 // 单圈值
float GetAngle_NoTrack(void) { float GetAngle_NoTrack(struct MT6701 *mt6701) {
Set_Ang_Sensor(mt6701->Mot_num);
MT6701_iic_read_angel(); MT6701_iic_read_angel();
angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2); angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
angle_f_rad = (float)angle * _2PI / 16384; angle_f_rad = (float)angle * _2PI / 16384;
@@ -169,52 +63,44 @@ float GetAngle_NoTrack(void) {
} }
return angle_f_rad; return angle_f_rad;
} }
volatile float full_rotations = 0.0;
volatile float Last_Angle_rad = 0.0;
// 多圈值 // 多圈值
float GetAngle(void) { float GetAngle(struct MT6701 *mt6701) {
volatile float D_Angle_rad = 0.0; // float D_Angle = 0.0;
volatile float Angle_rad = GetAngle_NoTrack(); mt6701->Angle = GetAngle_NoTrack(mt6701);
D_Angle_rad = Angle_rad - Last_Angle_rad; float D_Angle = mt6701->Angle - mt6701->Last_Angle;
if (fabs(D_Angle_rad) > (0.8f * 2 * PI)) { if (fabs(D_Angle) > (0.8f * 2 * PI)) {
full_rotations = full_rotations + ((D_Angle_rad > 0) ? -1 : 1); mt6701->full_rotations = mt6701->full_rotations + ((D_Angle > 0) ? -1 : 1);
} }
Last_Angle_rad = Angle_rad; mt6701->Last_Angle = mt6701->Angle;
return (full_rotations * 2 * PI + Last_Angle_rad); mt6701->Angle = (mt6701->full_rotations * _2PI + mt6701->Last_Angle);
return mt6701->Angle;
} }
volatile float Last_Vel_ts = 0.0; float GetVelocity(struct MT6701 *mt6701_Vel) {
volatile float Vel_Last_Angle = 0.0; float dt = 0.0;
float GetVelocity(void) { float Vel_ts = DL_SYSTICK_getValue();
volatile float dt = 0.0; if (Vel_ts < mt6701_Vel->Last_Vel_ts)
volatile float Vel_ts = SysTick->VAL; dt = (mt6701_Vel->Last_Vel_ts - Vel_ts) / MCLK_IN_MHZ * 1e-6f;
if (Vel_ts < Last_Vel_ts) { else
dt = (Last_Vel_ts - Vel_ts) / 80 * 1e-6f; dt = (RELOAD_CYCLES - Vel_ts + mt6701_Vel->Last_Vel_ts) / MCLK_IN_MHZ *
} else { 1e-6f;
dt = (0xFFFFFF - Vel_ts + Last_Vel_ts) / 80 * 1e-6f;
}
if (dt < 0.0001) { if (dt < 0.0001)
dt = 10000; dt = 10000;
}
float Vel_Angle = GetAngle(); float Vel_Angle = GetAngle(mt6701_Vel);
float dv = Vel_Angle - Vel_Last_Angle; float dv = Vel_Angle - mt6701_Vel->Vel_Last_Angle;
float velocity = (Vel_Angle - Vel_Last_Angle) / dt; mt6701_Vel->velocity = (Vel_Angle - mt6701_Vel->Vel_Last_Angle) / dt;
Last_Vel_ts = Vel_ts; mt6701_Vel->Last_Vel_ts = Vel_ts;
Vel_Last_Angle = Vel_Angle; mt6701_Vel->Vel_Last_Angle = Vel_Angle;
return velocity; return mt6701_Vel->velocity;
}
void MT6701_get_angle_degree(void) {
MT6701_iic_read_angel();
angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
angle_f = (float)angle * 360 / 16384;
} }

31
3rd/mt6701.h
View File

@@ -4,26 +4,31 @@
#include <stdint.h> #include <stdint.h>
#ifdef __cplusplus #ifdef __cplusplus
extern "C" extern "C" {
{
#endif #endif
#define PI 3.14159265359f #define PI 3.14159265359f
#define _2PI 6.28318530718f #define _2PI 6.28318530718f
#define I2C_TX_PACKET_SIZE (1) #define I2C_TX_PACKET_SIZE (1)
/*
* Number of bytes to received from target.
* This example uses FIFO with polling, and the maximum FIFO size is 8.
* Refer to interrupt examples to handle larger packets
*/
#define I2C_RX_PACKET_SIZE (2) #define I2C_RX_PACKET_SIZE (2)
struct MT6701 {
int Mot_num;
float Angle;
float velocity;
float full_rotations;
float Last_Angle;
float Last_Vel_ts;
float Vel_Last_Angle;
};
void Set_Sensor(int Mot);
void MT6701_iic_read_angel(void); void MT6701_iic_read_angel(void);
void MT6701_get_angle_degree(void); void MT6701_get_angle_degree(void);
float GetAngle(void); float GetAngle(struct MT6701 *mt6701);
float GetAngle_NoTrack(void); float GetAngle_NoTrack(struct MT6701 *mt6701);
float GetVelocity(void); float GetVelocity(struct MT6701 *mt6701_Vel);
#ifdef __cplusplus #ifdef __cplusplus
} }

73
3rd/pid_control.c
View File

@@ -1,51 +1,42 @@
#include "pid_control.h"
#include "config.h"
#include <stdint.h> #include <stdint.h>
#include <ti_msp_dl_config.h> #include <ti_msp_dl_config.h>
#define limit 5.0
#define Output_ramp 10000
//限幅 #define limit 5.0
float _constrain(float amt, float low, float high) #define Output_ramp 10000
{
return ((amt < low) ? (low) : ((amt) > (high) ? (high) : (amt))); // 限幅
float _constrain(float amt, float low, float high) {
return ((amt < low) ? (low) : ((amt) > (high) ? (high) : (amt)));
} }
unsigned long Timestamp_Last = 0.0; float PID_Controller(struct _PID *pid, float error) {
float Last_Error = 0.0; float Ts = 0.0;
float Last_intergration = 0.0; uint32_t Timestamp = DL_SYSTICK_getValue(); // 假设这里是正确获取时间戳的方式
float Last_Output = 0.0; if (Timestamp < pid->Timestamp_Last)
float PID_Controller(float Kp, float Ki, float Kd, float Error) Ts = (float)(pid->Timestamp_Last - Timestamp) / MCLK_IN_MHZ * 1e-6;
{ else
float Ts = 0.0; Ts = (0xFFFFFF - Timestamp + pid->Timestamp_Last) / MCLK_IN_MHZ * 1e-6;
uint32_t Timestamp = DL_SYSTICK_getValue();
if(Timestamp < Timestamp_Last)
{
Ts = (float)(Timestamp_Last - Timestamp) / 80 * 1e-6;
}
else
{
Ts = (0xFFFFFF - Timestamp + Timestamp_Last) / 80 * 1e-6;
}
if(Ts <= 0 || Ts > 0.05f) if (Ts <= 0 || Ts > 0.05f)
{ Ts = 0.001;
Ts = 0.001;
}
float proportion = Kp * Error;//P环 float proportion = pid->Kp * error; // P环
float intergration = Last_intergration + Ki * 0.5f * Ts * Error;//I环 float intergration =
intergration = _constrain(intergration, -limit, limit); pid->Last_intergration + pid->Ki * 0.5f * Ts * error; // I环
// 假设 _constrain 函数可以对 intergration 进行限制
intergration = _constrain(intergration, -limit, limit);
float differential = pid->Kd * (error - pid->Last_Error) / Ts; // D环
float differential = Kd * (Error - Last_Error) / Ts; //D环 float Output = proportion + intergration + differential;
// 假设 _constrain 函数可以对 Output 进行限制
float Output = proportion + intergration + differential; Output = _constrain(Output, -limit, limit);
Output = _constrain(Output, -limit, limit); pid->Last_Error = error;
pid->Last_intergration = intergration;
Last_Error = Error; pid->Last_Output = Output;
Last_intergration = intergration; pid->Timestamp_Last = Timestamp;
Last_Output = Output;
Timestamp_Last = Timestamp;
return Output;
}
return Output;
}

16
3rd/pid_control.h
View File

@@ -1,9 +1,17 @@
#ifndef __PID_CONTROL_H #ifndef __PID_CONTROL_H
#define __PID_CONTROL_H #define __PID_CONTROL_H
float PID_Controller(float Kp, float Ki, float Kd, float Error); struct _PID {
float _constrain(float amt, float low, float high); float Kp;
float Ki;
float Kd;
unsigned long Timestamp_Last;
float Last_Error;
float Last_intergration;
float Last_Output;
};
float PID_Controller(struct _PID *pid, float error);
float _constrain(float amt, float low, float high);
#endif #endif

23
3rd/pwm.c
View File

@@ -1,25 +1,20 @@
#include "pwm.h" #include "pwm.h"
#include "ti_msp_dl_config.h" #include "ti_msp_dl_config.h"
void PWM_Channel1(uint16_t Compare) void M0_PWM_A(float Compare) {
{ DL_TimerA_setCaptureCompareValue(PWM_0_INST, Compare, GPIO_PWM_0_C0_IDX);
DL_TimerA_setCaptureCompareValue(PWM_0_INST, Compare, GPIO_PWM_0_C0_IDX);
} }
void PWM_Channel2(uint16_t Compare) void M0_PWM_B(float Compare) {
{ DL_TimerA_setCaptureCompareValue(PWM_0_INST, Compare, GPIO_PWM_0_C1_IDX);
DL_TimerA_setCaptureCompareValue(PWM_0_INST, Compare, GPIO_PWM_0_C1_IDX);
} }
void PWM_Channel3(uint16_t Compare) void M0_PWM_C(float Compare) {
{ DL_TimerA_setCaptureCompareValue(PWM_0_INST, Compare, GPIO_PWM_0_C2_IDX);
DL_TimerA_setCaptureCompareValue(PWM_0_INST, Compare, GPIO_PWM_0_C2_IDX);
} }
void M1_PWM_A(float Compare) {}
void M1_PWM_B(float Compare) {}
void M1_PWM_C(float Compare) {}

12
3rd/pwm.h
View File

@@ -1,11 +1,11 @@
#ifndef __PWM_H #ifndef __PWM_H
#define __PWM_H #define __PWM_H
#include <stdint.h>
void PWM_Channel1(uint16_t Compare); void M0_PWM_A(float Compare);
void PWM_Channel2(uint16_t Compare); void M0_PWM_B(float Compare);
void PWM_Channel3(uint16_t Compare); void M0_PWM_C(float Compare);
void M1_PWM_A(float Compare);
void M1_PWM_B(float Compare);
void M1_PWM_C(float Compare);
#endif #endif

32
3rd/soft_i2c.c
View File

@@ -4,33 +4,47 @@
#define DELAY 1 #define DELAY 1
#define SDA_PIN_SHIFT 9 // if pa.8 shift8 , if pa.9 shift 9 #define SDA_PIN_SHIFT 9 // if pa.8 shift8 , if pa.9 shift 9
uint32_t SCL_PIN, SDA_PIN;
uint32_t SDA_IOMUX;
GPIO_Regs *PORT;
void Set_Ang_Sensor(int Mot) {
if (Mot == 0) {
PORT = SOFT_I2C_PORT;
SDA_IOMUX = SOFT_I2C_SDA_IOMUX;
SCL_PIN = SOFT_I2C_CLK_PIN;
SDA_PIN = SOFT_I2C_SDA_PIN;
} else if (Mot == 1) {
}
}
void I2C_ENABLE_OUTPUT_SDA(void) { void I2C_ENABLE_OUTPUT_SDA(void) {
DL_GPIO_initDigitalOutput(SOFT_I2C_SDA_IOMUX); DL_GPIO_initDigitalOutput(SDA_IOMUX);
DL_GPIO_enableOutput(SOFT_I2C_PORT, SOFT_I2C_SDA_PIN); DL_GPIO_enableOutput(PORT, SDA_PIN);
} }
void I2C_ENABLE_INPUT_SDA(void) { void I2C_ENABLE_INPUT_SDA(void) {
DL_GPIO_disableOutput(SOFT_I2C_PORT, SOFT_I2C_SDA_PIN); DL_GPIO_disableOutput(PORT, SDA_PIN);
DL_GPIO_initDigitalInputFeatures( DL_GPIO_initDigitalInputFeatures(
SOFT_I2C_SDA_IOMUX, DL_GPIO_INVERSION_DISABLE, DL_GPIO_RESISTOR_PULL_UP, SDA_IOMUX, DL_GPIO_INVERSION_DISABLE, DL_GPIO_RESISTOR_PULL_UP,
DL_GPIO_HYSTERESIS_DISABLE, DL_GPIO_WAKEUP_DISABLE); DL_GPIO_HYSTERESIS_DISABLE, DL_GPIO_WAKEUP_DISABLE);
} }
void I2C_W_SCL(uint8_t BitValue) { void I2C_W_SCL(uint8_t BitValue) {
if (BitValue) { if (BitValue) {
DL_GPIO_setPins(SOFT_I2C_PORT, SOFT_I2C_CLK_PIN); DL_GPIO_setPins(PORT, SCL_PIN);
} else { } else {
DL_GPIO_clearPins(SOFT_I2C_PORT, SOFT_I2C_CLK_PIN); DL_GPIO_clearPins(PORT, SCL_PIN);
} }
delay_us(DELAY); delay_us(DELAY);
} }
void I2C_W_SDA(uint8_t BitValue) { void I2C_W_SDA(uint8_t BitValue) {
if (BitValue) { if (BitValue) {
DL_GPIO_setPins(SOFT_I2C_PORT, SOFT_I2C_SDA_PIN); DL_GPIO_setPins(PORT, SDA_PIN);
} else { } else {
DL_GPIO_clearPins(SOFT_I2C_PORT, SOFT_I2C_SDA_PIN); DL_GPIO_clearPins(PORT, SDA_PIN);
} }
delay_us(DELAY); delay_us(DELAY);
} }
@@ -38,7 +52,7 @@ void I2C_W_SDA(uint8_t BitValue) {
// 读取时钟线数据 // 读取时钟线数据
uint8_t I2C_R_SDA(void) { uint8_t I2C_R_SDA(void) {
uint32_t BitValue; uint32_t BitValue;
BitValue = DL_GPIO_readPins(SOFT_I2C_PORT, SOFT_I2C_SDA_PIN); BitValue = DL_GPIO_readPins(PORT, SDA_PIN);
return (uint8_t)(BitValue >> 9); return (uint8_t)(BitValue >> 9);
} }

1
3rd/soft_i2c.h
View File

@@ -11,5 +11,6 @@ void I2C_SendByte(uint8_t Byte);
uint8_t I2C_RecviveData(void); uint8_t I2C_RecviveData(void);
void I2C_SendAck(uint8_t AckBit); void I2C_SendAck(uint8_t AckBit);
uint8_t I2C_RecviveAck(void); uint8_t I2C_RecviveAck(void);
void Set_Ang_Sensor(int Mot);
#endif /* ti_msp_dl_config_h */ #endif /* ti_msp_dl_config_h */

30
empty.c
View File

@@ -50,8 +50,14 @@ const float num_f = 0.123456f;
volatile uint16_t count = 0; volatile uint16_t count = 0;
volatile float Target = 20; // 串口目标值 volatile float Target = 20; // 串口目标值
const int pp = 7; // 电机极对数 int M0_PP = 7, M0_DIR = 1;
const int Dir = -1; // 电机编码器方向 int M1_PP = 7, M1_DIR = 1;
int Motor0 = 0;
int Motor1 = 1;
float M0_Target = 50.0; // 串口目标值
float M1_Target = 50.0; // 串口目标值
#define UART_PACKET_SIZE (6) #define UART_PACKET_SIZE (6)
@@ -108,25 +114,17 @@ int main(void) {
NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN); NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN);
DL_TimerG_startCounter(TIMER_0_INST); DL_TimerG_startCounter(TIMER_0_INST);
FOC_Init(12.6); FOC_Init();
DL_SYSTICK_resetValue(); DL_SYSTICK_resetValue();
while (1) { while (1) {
// DL_GPIO_togglePins(LED_PORT, LED_PA0_PIN); M0_Set_Velocity(M0_Target); // 速度模式
// delay_ms(10); Print_Velocity(0);
// 开环
velocityopenloop(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();
M0_Target = Target;
} }
} }
} }
@@ -135,8 +133,8 @@ void TIMER_0_INST_IRQHandler(void) {
switch (DL_TimerA_getPendingInterrupt(TIMER_0_INST)) { switch (DL_TimerA_getPendingInterrupt(TIMER_0_INST)) {
case DL_TIMERG_INTERRUPT_ZERO_EVENT: case DL_TIMERG_INTERRUPT_ZERO_EVENT:
printf("Target is %f \n", Target); // printf("Target is %f \n", Target);
// printf("%s",gUartRxPacket); // printf("%s",gUartRxPacket);
DL_GPIO_togglePins(LED_PORT, LED_PA0_PIN); DL_GPIO_togglePins(LED_PORT, LED_PA0_PIN);
break; break;