diff --git a/3rd/config.h b/3rd/config.h index e6fc993..3426fd0 100644 --- a/3rd/config.h +++ b/3rd/config.h @@ -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 \ No newline at end of file diff --git a/3rd/dfoc.c b/3rd/dfoc.c index f387ace..3363583 100644 --- a/3rd/dfoc.c +++ b/3rd/dfoc.c @@ -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)); } } diff --git a/3rd/lowpass_filter.c b/3rd/lowpass_filter.c index 219c408..54801e2 100644 --- a/3rd/lowpass_filter.c +++ b/3rd/lowpass_filter.c @@ -1,5 +1,6 @@ #include "lowpass_filter.h" +#include "config.h" #include #include @@ -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; diff --git a/3rd/mt6701.c b/3rd/mt6701.c index dc79d77..d6d8104 100644 --- a/3rd/mt6701.c +++ b/3rd/mt6701.c @@ -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; } diff --git a/3rd/mt6701.h b/3rd/mt6701.h index b37b299..ff3ba13 100644 --- a/3rd/mt6701.h +++ b/3rd/mt6701.h @@ -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 } diff --git a/3rd/pid_control.c b/3rd/pid_control.c index 121d07c..e94892e 100644 --- a/3rd/pid_control.c +++ b/3rd/pid_control.c @@ -1,4 +1,5 @@ #include "pid_control.h" +#include "config.h" #include #include @@ -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; diff --git a/empty.c b/empty.c index a31c37c..a9f2511 100644 --- a/empty.c +++ b/empty.c @@ -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;