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

correct SYSTICK and RELOAD_CYCLES

Browse Source
This commit is contained in:
4x-tech
2025-11-20 16:32:17 +08:00
parent a9aa2e19c8
commit a2d2773ace
7 changed files with 50 additions and 147 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
3rd/config.h
View File

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

10
3rd/dfoc.c
View File

@@ -24,8 +24,8 @@ struct _PID M1_VEL_PID = {0.4, 2, 0};
struct LOWPASS M0_VEL_Filter = {0.1};
struct LOWPASS M1_VEL_Filter = {0.1};
struct AS5600_Sensor Angle_Sensor0 = {0};
struct AS5600_Sensor Angle_Sensor1 = {1};
struct MT6701 Angle_Sensor0 = {0};
struct MT6701 Angle_Sensor1 = {1};
#define VOLTAGE_POWER_SUPPLY 12.0f
#define VOLTAGE_LIMIT 6
@@ -123,16 +123,16 @@ void Check_Sensor(void) {
void FOC_Init() {
// PWM_Init();
// CurrSense_Init();
// AS5600_Init();
// mt6701_Init();
Check_Sensor();
}
void Print_Velocity(int Motor_Velocity) {
if (Motor_Velocity == 0) {
printf("Velocity M0 is %f \n", GetVelocity(&Angle_Sensor0));
printf("M0:%f\n", GetVelocity(&Angle_Sensor0));
}
if (Motor_Velocity == 1) {
printf("Velocity M1 is %f \n", GetVelocity(&Angle_Sensor1));
printf("M1:%f\n", GetVelocity(&Angle_Sensor1));
}
}

6
3rd/lowpass_filter.c
View File

@@ -1,5 +1,6 @@
#include "lowpass_filter.h"
#include "config.h"
#include <stdint.h>
#include <ti_msp_dl_config.h>
@@ -17,9 +18,10 @@ float Lowpassfilter(struct LOWPASS *lowpass, float x) {
uint32_t Timesamp = DL_SYSTICK_getValue();
if (Timesamp < lowpass->Last_Timesamp)
dt = (float)(lowpass->Last_Timesamp - Timesamp) / 9 * 1e-6;
dt = (float)(lowpass->Last_Timesamp - Timesamp) / MCLK_IN_MHZ * 1e-6;
else
dt = (float)(0xFFFFFF - Timesamp + lowpass->Last_Timesamp) / 9 * 1e-6;
dt = (float)(RELOAD_CYCLES - Timesamp + lowpass->Last_Timesamp) /
MCLK_IN_MHZ * 1e-6;
if (dt < 0.0 || dt == 0)
dt = 0.0015f;

146
3rd/mt6701.c
View File

@@ -52,116 +52,9 @@ void MT6701_iic_read_angel(void) {
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(struct AS5600_Sensor *AS5600) {
Set_Ang_Sensor(AS5600->Mot_num);
float GetAngle_NoTrack(struct MT6701 *mt6701) {
Set_Ang_Sensor(mt6701->Mot_num);
MT6701_iic_read_angel();
angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
angle_f_rad = (float)angle * _2PI / 16384;
@@ -172,41 +65,42 @@ float GetAngle_NoTrack(struct AS5600_Sensor *AS5600) {
}
// 多圈值
float GetAngle(struct AS5600_Sensor *AS5600) {
float GetAngle(struct MT6701 *mt6701) {
// float D_Angle = 0.0;
AS5600->Angle = GetAngle_NoTrack(AS5600);
float D_Angle = AS5600->Angle - AS5600->Last_Angle;
mt6701->Angle = GetAngle_NoTrack(mt6701);
float D_Angle = mt6701->Angle - mt6701->Last_Angle;
if (fabs(D_Angle) > (0.8f * 2 * PI)) {
AS5600->full_rotations = AS5600->full_rotations + ((D_Angle > 0) ? -1 : 1);
mt6701->full_rotations = mt6701->full_rotations + ((D_Angle > 0) ? -1 : 1);
}
AS5600->Last_Angle = AS5600->Angle;
mt6701->Last_Angle = mt6701->Angle;
AS5600->Angle = (AS5600->full_rotations * _2PI + AS5600->Last_Angle);
mt6701->Angle = (mt6701->full_rotations * _2PI + mt6701->Last_Angle);
return AS5600->Angle;
return mt6701->Angle;
}
float GetVelocity(struct AS5600_Sensor *AS5600_Vel) {
float GetVelocity(struct MT6701 *mt6701_Vel) {
float dt = 0.0;
float Vel_ts = DL_SYSTICK_getValue();
if (Vel_ts < AS5600_Vel->Last_Vel_ts)
dt = (AS5600_Vel->Last_Vel_ts - Vel_ts) / 9 * 1e-6f;
if (Vel_ts < mt6701_Vel->Last_Vel_ts)
dt = (mt6701_Vel->Last_Vel_ts - Vel_ts) / MCLK_IN_MHZ * 1e-6f;
else
dt = (0xFFFFFF - Vel_ts + AS5600_Vel->Last_Vel_ts) / 9 * 1e-6f;
dt = (RELOAD_CYCLES - Vel_ts + mt6701_Vel->Last_Vel_ts) / MCLK_IN_MHZ *
1e-6f;
if (dt < 0.0001)
dt = 10000;
float Vel_Angle = GetAngle(AS5600_Vel);
float Vel_Angle = GetAngle(mt6701_Vel);
float dv = Vel_Angle - AS5600_Vel->Vel_Last_Angle;
float dv = Vel_Angle - mt6701_Vel->Vel_Last_Angle;
AS5600_Vel->velocity = (Vel_Angle - AS5600_Vel->Vel_Last_Angle) / dt;
mt6701_Vel->velocity = (Vel_Angle - mt6701_Vel->Vel_Last_Angle) / dt;
AS5600_Vel->Last_Vel_ts = Vel_ts;
AS5600_Vel->Vel_Last_Angle = Vel_Angle;
mt6701_Vel->Last_Vel_ts = Vel_ts;
mt6701_Vel->Vel_Last_Angle = Vel_Angle;
return AS5600_Vel->velocity;
return mt6701_Vel->velocity;
}

8
3rd/mt6701.h
View File

@@ -13,7 +13,7 @@ extern "C" {
#define I2C_TX_PACKET_SIZE (1)
#define I2C_RX_PACKET_SIZE (2)
struct AS5600_Sensor {
struct MT6701 {
int Mot_num;
float Angle;
float velocity;
@@ -26,9 +26,9 @@ struct AS5600_Sensor {
void Set_Sensor(int Mot);
void MT6701_iic_read_angel(void);
void MT6701_get_angle_degree(void);
float GetAngle(struct AS5600_Sensor *AS5600);
float GetAngle_NoTrack(struct AS5600_Sensor *AS5600);
float GetVelocity(struct AS5600_Sensor *AS5600_Vel);
float GetAngle(struct MT6701 *mt6701);
float GetAngle_NoTrack(struct MT6701 *mt6701);
float GetVelocity(struct MT6701 *mt6701_Vel);
#ifdef __cplusplus
}

5
3rd/pid_control.c
View File

@@ -1,4 +1,5 @@
#include "pid_control.h"
#include "config.h"
#include <stdint.h>
#include <ti_msp_dl_config.h>
@@ -14,9 +15,9 @@ float PID_Controller(struct _PID *pid, float error) {
float Ts = 0.0;
uint32_t Timestamp = DL_SYSTICK_getValue(); // 假设这里是正确获取时间戳的方式
if (Timestamp < pid->Timestamp_Last)
Ts = (float)(pid->Timestamp_Last - Timestamp) / 9 * 1e-6;
Ts = (float)(pid->Timestamp_Last - Timestamp) / MCLK_IN_MHZ * 1e-6;
else
Ts = (0xFFFFFF - Timestamp + pid->Timestamp_Last) / 9 * 1e-6;
Ts = (0xFFFFFF - Timestamp + pid->Timestamp_Last) / MCLK_IN_MHZ * 1e-6;
if (Ts <= 0 || Ts > 0.05f)
Ts = 0.001;

12
empty.c
View File

@@ -56,8 +56,8 @@ int M1_PP = 7, M1_DIR = 1;
int Motor0 = 0;
int Motor1 = 1;
float M0_Target = 10.0; // 串口目标值
float M1_Target = 20.0; // 串口目标值
float M0_Target = 50.0; // 串口目标值
float M1_Target = 50.0; // 串口目标值
#define UART_PACKET_SIZE (6)
@@ -119,10 +119,12 @@ int main(void) {
while (1) {
M0_Set_Velocity(M0_Target); // 速度模式
Print_Velocity(0);
if (gCheckUART) {
gCheckUART = false;
parse_uart_cmd();
M0_Target = Target;
}
}
}
@@ -131,10 +133,8 @@ void TIMER_0_INST_IRQHandler(void) {
switch (DL_TimerA_getPendingInterrupt(TIMER_0_INST)) {
case DL_TIMERG_INTERRUPT_ZERO_EVENT:
printf("Target is %f \n", Target);
M0_Target = Target;
// printf("%s",gUartRxPacket);
Print_Velocity(0);
// printf("Target is %f \n", Target);
// printf("%s",gUartRxPacket);
DL_GPIO_togglePins(LED_PORT, LED_PA0_PIN);
break;