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

chenged to 2 motor velocity-closed-loop; before testing

Browse Source
This commit is contained in:
4x-tech
2025-11-20 09:50:13 +08:00
parent dd8f7c006f
commit ed09f10c03
13 changed files with 276 additions and 261 deletions
Download Patch File Download Diff File Expand all files Collapse all files

156
3rd/dfoc.c
View File

@@ -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());
}

35
3rd/dfoc.h
View File

@@ -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

62
3rd/lowpass_filter.c
View File

@@ -1,48 +1,38 @@
#include "lowpass_filter.h"
#include <stdint.h>
#include <ti_msp_dl_config.h>
#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;
}

11
3rd/lowpass_filter.h
View File

@@ -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

62
3rd/mt6701.c
View File

@@ -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;
}

31
3rd/mt6701.h
View File

@@ -4,26 +4,31 @@
#include <stdint.h>
#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
}

72
3rd/pid_control.c
View File

@@ -1,51 +1,41 @@
#include "pid_control.h"
#include <stdint.h>
#include <ti_msp_dl_config.h>
#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;
}

16
3rd/pid_control.h
View File

@@ -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

23
3rd/pwm.c
View File

@@ -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) {}

12
3rd/pwm.h
View File

@@ -1,11 +1,11 @@
#ifndef __PWM_H
#define __PWM_H
#include <stdint.h>
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

32
3rd/soft_i2c.c
View File

@@ -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);
}

1
3rd/soft_i2c.h
View File

@@ -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 */

24
empty.c
View File

@@ -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();