From ed09f10c0394460da8d5f2cf5b6da7db6ec81927 Mon Sep 17 00:00:00 2001 From: 4x-tech Date: Thu, 20 Nov 2025 09:50:13 +0800 Subject: [PATCH] chenged to 2 motor velocity-closed-loop; before testing --- 3rd/dfoc.c | 156 +++++++++++++++++++++++-------------------- 3rd/dfoc.h | 35 +++++++--- 3rd/lowpass_filter.c | 62 ++++++++--------- 3rd/lowpass_filter.h | 11 +-- 3rd/mt6701.c | 62 ++++++++--------- 3rd/mt6701.h | 31 +++++---- 3rd/pid_control.c | 72 +++++++++----------- 3rd/pid_control.h | 16 +++-- 3rd/pwm.c | 23 +++---- 3rd/pwm.h | 12 ++-- 3rd/soft_i2c.c | 32 ++++++--- 3rd/soft_i2c.h | 1 + empty.c | 24 +++---- 13 files changed, 276 insertions(+), 261 deletions(-) diff --git a/3rd/dfoc.c b/3rd/dfoc.c index 90e7030..49b6517 100644 --- a/3rd/dfoc.c +++ b/3rd/dfoc.c @@ -1,4 +1,5 @@ +#include "dfoc.h" #include "config.h" #include "delay.h" #include "lowpass_filter.h" @@ -15,6 +16,18 @@ #define _1_SQRT3 0.57735026919f #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 AS5600_Sensor Angle_Sensor0 = {0}; +struct AS5600_Sensor Angle_Sensor1 = {1}; + #define VOLTAGE_POWER_SUPPLY 12.0f #define VOLTAGE_LIMIT 6 #define PWM_COMPARE_VALUE 750.0f @@ -22,10 +35,10 @@ volatile float Ua = 0, Ub = 0, Uc = 0, Ualpha, Ubeta = 0, dc_a = 0, dc_b = 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 Dir; +extern int M0_PP, M0_DIR; +extern int M1_PP, M1_DIR; void Motor_en() {} @@ -40,11 +53,16 @@ float normalizeAngle(float angle) { return ((a >= 0) ? a : (a + 2 * PI)); } -float electricAngle(void) { - return normalizeAngle((GetAngle_NoTrack() * pp * Dir) - zero_electric_Angle); +float M0_electricAngle(void) { + 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_b = 0.0; volatile float U_c = 0.0; @@ -53,44 +71,54 @@ void SetPwm(float Ua, float Ub, float Uc) { U_b = constrain(Ub, 0.0f, VOLTAGE_LIMIT); U_c = constrain(Uc, 0.0f, VOLTAGE_LIMIT); - 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); - if (DEBUG_ENABLED & DEBUG_DFOC_ENABLED) { + if (Mot_num == 0) { + M0.dc_a = constrain(U_a / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f); + M0.dc_b = constrain(U_b / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f); + 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核心算法，克拉克逆变换/帕克逆变换 -volatile float test_angle = 0.0; -volatile float last_test_angle = 0.0; -void SetPhaseVoltage(float Uq, float Ud, float angle_el) { + +void SetPhaseVoltage(struct Motor_ *Motor, float Uq, float angle_el) { // angle_el = normalizeAngle(angle_el); - test_angle = angle_el - last_test_angle; - Ualpha = -Uq * sin(angle_el); - Ubeta = Uq * cos(angle_el); + Motor->Ualpha = -Uq * sin(angle_el); + Motor->Ubeta = Uq * cos(angle_el); - Ua = Ualpha + VOLTAGE_POWER_SUPPLY / 2; - Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + VOLTAGE_POWER_SUPPLY / 2; - Uc = -(Ualpha + sqrt(3) * Ubeta) / 2 + VOLTAGE_POWER_SUPPLY / 2; + Motor->Ua = Motor->Ualpha + VOLTAGE_POWER_SUPPLY / 2; + Motor->Ub = + (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); - - last_test_angle = angle_el; + SetPwm(Motor->Mot_num, Motor->Ua, Motor->Ub, Motor->Uc); } void Check_Sensor(void) { - // SetPhaseVoltage(3, 0, _3PI_2); - // delay_ms(3000); - zero_electric_Angle = electricAngle(); - printf("Check_Sensor zero_electric_Angle is %f \n", zero_electric_Angle); - // SetPhaseVoltage(0, 0, _3PI_2); - // delay_ms(500); + SetPhaseVoltage(&M0, 3, _3PI_2); + delay_ms(2000); + M0_zero_elc_Angle = M0_electricAngle(); + SetPhaseVoltage(&M0, 0, _3PI_2); + 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() { @@ -100,50 +128,30 @@ void FOC_Init() { Check_Sensor(); } -// 单角度环 -void Set_Angle(float Target) { - volatile float langle = GetAngle(); - if (DEBUG_ENABLED & DEBUG_DFOC_ENABLED) { - printf("angle readback in dfoc.c is %f \n", langle); +void Print_Velocity(int Motor_Velocity) { + if (Motor_Velocity == 0) { + printf("Velocity M0 is %f \n", GetVelocity(&Angle_Sensor0)); } - volatile float Uq = - PID_Controller(0.03, 0.01, 0, (Target - Dir * langle) * 180 / PI); - // 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); + if (Motor_Velocity == 1) { + printf("Velocity M1 is %f \n", GetVelocity(&Angle_Sensor1)); } - SetPhaseVoltage(Uq, 0, electricAngle()); } -volatile double openloop_timestamp; -float velocityopenloop(float target) { - volatile float Uq = 0.0; - volatile double Ts = 0.0; - volatile double shaft_angle; - - 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 (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 M0_Set_Velocity(float Target) { + Angle_Sensor0.velocity = + Lowpassfilter(&M0_VEL_Filter, GetVelocity(&Angle_Sensor0)); + SetPhaseVoltage( + &M0, + PID_Controller(&M0_VEL_PID, M0_DIR * (Target - Angle_Sensor0.velocity)), + M0_electricAngle()); } + +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()); +} \ No newline at end of file diff --git a/3rd/dfoc.h b/3rd/dfoc.h index bba2692..a4bfa7a 100644 --- a/3rd/dfoc.h +++ b/3rd/dfoc.h @@ -1,20 +1,35 @@ #ifndef __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); 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); -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 FOC_Init(float power); -float electricAngle(void); -float GetCommand(void); -void Set_Angle(float Target); +void FOC_Init(void); +float M0_electricAngle(void); +float M1_electricAngle(void); +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); - - +void Print_Velocity(int Motor_Velocity); #endif - - diff --git a/3rd/lowpass_filter.c b/3rd/lowpass_filter.c index 82c95fc..219c408 100644 --- a/3rd/lowpass_filter.c +++ b/3rd/lowpass_filter.c @@ -1,48 +1,38 @@ +#include "lowpass_filter.h" #include #include +#define _2PI 6.28318530718f + float y = 0; -float Lowpassfilter_sim(float x) -{ - float out = 0.9 * x + 0.1 * y; - y = x; - return out; +float Lowpassfilter_sim(float x) { + float out = 0.9 * x + 0.1 * y; + y = x; + return out; } -uint32_t Last_Timesamp = 0.0; -float Last_y = 0.0; -float Lowpassfilter(float Tf, float x) -{ - float dt = 0.0; +float Lowpassfilter(struct LOWPASS *lowpass, float x) { + float dt = 0.0; - uint32_t Timesamp = DL_SYSTICK_getValue(); - if(Timesamp < Last_Timesamp) - { - dt = (float)(Last_Timesamp - Timesamp) / 80 * 1e-6; - } - else - { - dt = (float)(0xFFFFFF - Timesamp + Last_Timesamp) / 80 * 1e-6; - } + uint32_t Timesamp = DL_SYSTICK_getValue(); + if (Timesamp < lowpass->Last_Timesamp) + dt = (float)(lowpass->Last_Timesamp - Timesamp) / 9 * 1e-6; + else + dt = (float)(0xFFFFFF - Timesamp + lowpass->Last_Timesamp) / 9 * 1e-6; - if(dt < 0.0 || dt == 0) - { - dt = 0.0015f; - } - else if(dt > 0.005f) - { - Last_y = x; - Last_Timesamp = Timesamp; - return x; - } - float alpha = Tf / (Tf + dt); - float y = alpha * Last_y + (1.0f - alpha) * x; + if (dt < 0.0 || dt == 0) + dt = 0.0015f; + else if (dt > 0.005f) { + lowpass->Last_y = x; + lowpass->Last_Timesamp = Timesamp; + return x; + } + float alpha = lowpass->Tf / (lowpass->Tf + dt); + float y = alpha * lowpass->Last_y + (1.0f - alpha) * x; - Last_y = y; - Last_Timesamp = Timesamp; + lowpass->Last_y = y; + lowpass->Last_Timesamp = Timesamp; - - return y; + return y; } - diff --git a/3rd/lowpass_filter.h b/3rd/lowpass_filter.h index 4189639..cd8cd1b 100644 --- a/3rd/lowpass_filter.h +++ b/3rd/lowpass_filter.h @@ -1,12 +1,13 @@ #ifndef __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(struct LOWPASS *lowpass, float x); #endif - - diff --git a/3rd/mt6701.c b/3rd/mt6701.c index ae9076d..dc79d77 100644 --- a/3rd/mt6701.c +++ b/3rd/mt6701.c @@ -160,7 +160,8 @@ volatile float Last_ts = 0.0; volatile float last_angle = 0.0; // 单圈值 -float GetAngle_NoTrack(void) { +float GetAngle_NoTrack(struct AS5600_Sensor *AS5600) { + Set_Ang_Sensor(AS5600->Mot_num); MT6701_iic_read_angel(); angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2); angle_f_rad = (float)angle * _2PI / 16384; @@ -169,52 +170,43 @@ float GetAngle_NoTrack(void) { } return angle_f_rad; } -volatile float full_rotations = 0.0; -volatile float Last_Angle_rad = 0.0; + // 多圈值 -float GetAngle(void) { - volatile float D_Angle_rad = 0.0; - volatile float Angle_rad = GetAngle_NoTrack(); - D_Angle_rad = Angle_rad - Last_Angle_rad; +float GetAngle(struct AS5600_Sensor *AS5600) { + // float D_Angle = 0.0; + AS5600->Angle = GetAngle_NoTrack(AS5600); + float D_Angle = AS5600->Angle - AS5600->Last_Angle; - if (fabs(D_Angle_rad) > (0.8f * 2 * PI)) { - full_rotations = full_rotations + ((D_Angle_rad > 0) ? -1 : 1); + if (fabs(D_Angle) > (0.8f * 2 * PI)) { + AS5600->full_rotations = AS5600->full_rotations + ((D_Angle > 0) ? -1 : 1); } - Last_Angle_rad = Angle_rad; + AS5600->Last_Angle = AS5600->Angle; - return (full_rotations * 2 * PI + Last_Angle_rad); + AS5600->Angle = (AS5600->full_rotations * _2PI + AS5600->Last_Angle); + + return AS5600->Angle; } -volatile float Last_Vel_ts = 0.0; -volatile float Vel_Last_Angle = 0.0; -float GetVelocity(void) { - volatile float dt = 0.0; - volatile float Vel_ts = SysTick->VAL; - if (Vel_ts < Last_Vel_ts) { - dt = (Last_Vel_ts - Vel_ts) / 80 * 1e-6f; - } else { - dt = (0xFFFFFF - Vel_ts + Last_Vel_ts) / 80 * 1e-6f; - } +float GetVelocity(struct AS5600_Sensor *AS5600_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; + else + dt = (0xFFFFFF - Vel_ts + AS5600_Vel->Last_Vel_ts) / 9 * 1e-6f; - if (dt < 0.0001) { + if (dt < 0.0001) dt = 10000; - } - float Vel_Angle = GetAngle(); + float Vel_Angle = GetAngle(AS5600_Vel); - float dv = Vel_Angle - Vel_Last_Angle; + float dv = Vel_Angle - AS5600_Vel->Vel_Last_Angle; - float velocity = (Vel_Angle - Vel_Last_Angle) / dt; + AS5600_Vel->velocity = (Vel_Angle - AS5600_Vel->Vel_Last_Angle) / dt; - Last_Vel_ts = Vel_ts; - Vel_Last_Angle = Vel_Angle; + AS5600_Vel->Last_Vel_ts = Vel_ts; + AS5600_Vel->Vel_Last_Angle = Vel_Angle; - return 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; + return AS5600_Vel->velocity; } diff --git a/3rd/mt6701.h b/3rd/mt6701.h index 468a809..b37b299 100644 --- a/3rd/mt6701.h +++ b/3rd/mt6701.h @@ -4,26 +4,31 @@ #include #ifdef __cplusplus -extern "C" -{ +extern "C" { #endif -#define PI 3.14159265359f -#define _2PI 6.28318530718f +#define PI 3.14159265359f +#define _2PI 6.28318530718f #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) + +struct AS5600_Sensor { + 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_get_angle_degree(void); -float GetAngle(void); -float GetAngle_NoTrack(void); -float GetVelocity(void); +float GetAngle(struct AS5600_Sensor *AS5600); +float GetAngle_NoTrack(struct AS5600_Sensor *AS5600); +float GetVelocity(struct AS5600_Sensor *AS5600_Vel); #ifdef __cplusplus } diff --git a/3rd/pid_control.c b/3rd/pid_control.c index 8c11faa..121d07c 100644 --- a/3rd/pid_control.c +++ b/3rd/pid_control.c @@ -1,51 +1,41 @@ +#include "pid_control.h" #include #include -#define limit 5.0 -#define Output_ramp 10000 -//限幅 -float _constrain(float amt, float low, float high) -{ - return ((amt < low) ? (low) : ((amt) > (high) ? (high) : (amt))); +#define limit 5.0 +#define Output_ramp 10000 + +// 限幅 +float _constrain(float amt, float low, float high) { + return ((amt < low) ? (low) : ((amt) > (high) ? (high) : (amt))); } -unsigned long Timestamp_Last = 0.0; -float Last_Error = 0.0; -float Last_intergration = 0.0; -float Last_Output = 0.0; -float PID_Controller(float Kp, float Ki, float Kd, float Error) -{ - float Ts = 0.0; - 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; - } +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; + else + Ts = (0xFFFFFF - Timestamp + pid->Timestamp_Last) / 9 * 1e-6; - if(Ts <= 0 || Ts > 0.05f) - { - Ts = 0.001; - } + if (Ts <= 0 || Ts > 0.05f) + Ts = 0.001; - float proportion = Kp * Error;//P环 + float proportion = pid->Kp * error; // P环 - float intergration = Last_intergration + Ki * 0.5f * Ts * Error;//I环 - intergration = _constrain(intergration, -limit, limit); + float intergration = + 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; - Output = _constrain(Output, -limit, limit); - - Last_Error = Error; - Last_intergration = intergration; - Last_Output = Output; - Timestamp_Last = Timestamp; - - return Output; -} + float Output = proportion + intergration + differential; + // 假设 _constrain 函数可以对 Output 进行限制 + Output = _constrain(Output, -limit, limit); + pid->Last_Error = error; + pid->Last_intergration = intergration; + pid->Last_Output = Output; + pid->Timestamp_Last = Timestamp; + return Output; +} \ No newline at end of file diff --git a/3rd/pid_control.h b/3rd/pid_control.h index 04155c1..47f8187 100644 --- a/3rd/pid_control.h +++ b/3rd/pid_control.h @@ -1,9 +1,17 @@ #ifndef __PID_CONTROL_H #define __PID_CONTROL_H -float PID_Controller(float Kp, float Ki, float Kd, float Error); -float _constrain(float amt, float low, float high); +struct _PID { + 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 - - diff --git a/3rd/pwm.c b/3rd/pwm.c index 42d1d19..aeed541 100644 --- a/3rd/pwm.c +++ b/3rd/pwm.c @@ -1,25 +1,20 @@ #include "pwm.h" #include "ti_msp_dl_config.h" -void PWM_Channel1(uint16_t Compare) -{ - DL_TimerA_setCaptureCompareValue(PWM_0_INST, Compare, GPIO_PWM_0_C0_IDX); +void M0_PWM_A(float Compare) { + DL_TimerA_setCaptureCompareValue(PWM_0_INST, Compare, GPIO_PWM_0_C0_IDX); } -void PWM_Channel2(uint16_t Compare) -{ - DL_TimerA_setCaptureCompareValue(PWM_0_INST, Compare, GPIO_PWM_0_C1_IDX); +void M0_PWM_B(float Compare) { + DL_TimerA_setCaptureCompareValue(PWM_0_INST, Compare, GPIO_PWM_0_C1_IDX); } -void PWM_Channel3(uint16_t Compare) -{ - DL_TimerA_setCaptureCompareValue(PWM_0_INST, Compare, GPIO_PWM_0_C2_IDX); +void M0_PWM_C(float Compare) { + 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) {} diff --git a/3rd/pwm.h b/3rd/pwm.h index 2eb0e99..ba63518 100644 --- a/3rd/pwm.h +++ b/3rd/pwm.h @@ -1,11 +1,11 @@ #ifndef __PWM_H #define __PWM_H -#include -void PWM_Channel1(uint16_t Compare); -void PWM_Channel2(uint16_t Compare); -void PWM_Channel3(uint16_t Compare); +void M0_PWM_A(float Compare); +void M0_PWM_B(float 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 - - diff --git a/3rd/soft_i2c.c b/3rd/soft_i2c.c index da900d1..47bd29d 100644 --- a/3rd/soft_i2c.c +++ b/3rd/soft_i2c.c @@ -4,33 +4,47 @@ #define DELAY 1 #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) { - DL_GPIO_initDigitalOutput(SOFT_I2C_SDA_IOMUX); - DL_GPIO_enableOutput(SOFT_I2C_PORT, SOFT_I2C_SDA_PIN); + DL_GPIO_initDigitalOutput(SDA_IOMUX); + DL_GPIO_enableOutput(PORT, SDA_PIN); } 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( - 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); } void I2C_W_SCL(uint8_t BitValue) { if (BitValue) { - DL_GPIO_setPins(SOFT_I2C_PORT, SOFT_I2C_CLK_PIN); + DL_GPIO_setPins(PORT, SCL_PIN); } else { - DL_GPIO_clearPins(SOFT_I2C_PORT, SOFT_I2C_CLK_PIN); + DL_GPIO_clearPins(PORT, SCL_PIN); } delay_us(DELAY); } void I2C_W_SDA(uint8_t BitValue) { if (BitValue) { - DL_GPIO_setPins(SOFT_I2C_PORT, SOFT_I2C_SDA_PIN); + DL_GPIO_setPins(PORT, SDA_PIN); } else { - DL_GPIO_clearPins(SOFT_I2C_PORT, SOFT_I2C_SDA_PIN); + DL_GPIO_clearPins(PORT, SDA_PIN); } delay_us(DELAY); } @@ -38,7 +52,7 @@ void I2C_W_SDA(uint8_t BitValue) { // 读取时钟线数据 uint8_t I2C_R_SDA(void) { 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); } diff --git a/3rd/soft_i2c.h b/3rd/soft_i2c.h index ca310a2..5dc13fe 100644 --- a/3rd/soft_i2c.h +++ b/3rd/soft_i2c.h @@ -11,5 +11,6 @@ void I2C_SendByte(uint8_t Byte); uint8_t I2C_RecviveData(void); void I2C_SendAck(uint8_t AckBit); uint8_t I2C_RecviveAck(void); +void Set_Ang_Sensor(int Mot); #endif /* ti_msp_dl_config_h */ \ No newline at end of file diff --git a/empty.c b/empty.c index 6826d6f..d5fdb5a 100644 --- a/empty.c +++ b/empty.c @@ -50,8 +50,14 @@ const float num_f = 0.123456f; volatile uint16_t count = 0; volatile float Target = 20; // 串口目标值 -const int pp = 7; // 电机极对数 -const int Dir = -1; // 电机编码器方向 +int M0_PP = 7, M0_DIR = 1; +int M1_PP = 7, M1_DIR = 1; + +int Motor0 = 0; +int Motor1 = 1; + +float M0_Target = 20.0; // 串口目标值 +float M1_Target = 20.0; // 串口目标值 #define UART_PACKET_SIZE (6) @@ -108,22 +114,12 @@ int main(void) { NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN); DL_TimerG_startCounter(TIMER_0_INST); - FOC_Init(12.6); + FOC_Init(); DL_SYSTICK_resetValue(); while (1) { - // DL_GPIO_togglePins(LED_PORT, LED_PA0_PIN); - // delay_ms(10); + M0_Set_Velocity(M0_Target); // 速度模式 - // 开环 - velocityopenloop(Target); - - // //test MT6701 - // MT6701_get_angle_degree(); - // printf("angle degree is %f \n", angle_f); - - // // 闭环 - // Set_Angle(Target); if (gCheckUART) { gCheckUART = false; parse_uart_cmd();