correct SYSTICK and RELOAD_CYCLES

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

3rd/dfoc.c

@@ -1,4 +1,4 @@
-
+#include "dfoc.h"
 #include "config.h"
 #include "delay.h"
 #include "lowpass_filter.h"
@@ -15,6 +15,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 MT6701 Angle_Sensor0 = {0};
+struct MT6701 Angle_Sensor1 = {1};
+
 #define VOLTAGE_POWER_SUPPLY 12.0f
 #define VOLTAGE_LIMIT 6
 #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,
                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 +52,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,97 +70,87 @@ 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() {
   // PWM_Init();
   // CurrSense_Init();
-  // AS5600_Init();
+  // mt6701_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("M0:%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("M1:%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());
+}

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

3rd/lowpass_filter.c

@@ -1,48 +1,40 @@
 
+#include "lowpass_filter.h"
+#include "config.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) / MCLK_IN_MHZ * 1e-6;
+  else
+    dt = (float)(RELOAD_CYCLES - Timesamp + lowpass->Last_Timesamp) /
+         MCLK_IN_MHZ * 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;
 }
-

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

3rd/mt6701.c

@@ -52,115 +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(void) {
+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;
@@ -169,52 +63,44 @@ 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 MT6701 *mt6701) {
+  //	float D_Angle = 0.0;
+  mt6701->Angle = GetAngle_NoTrack(mt6701);
+  float D_Angle = mt6701->Angle - mt6701->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)) {
+    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;
-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 MT6701 *mt6701_Vel) {
+  float dt = 0.0;
+  float Vel_ts = DL_SYSTICK_getValue();
+  if (Vel_ts < mt6701_Vel->Last_Vel_ts)
+    dt = (mt6701_Vel->Last_Vel_ts - Vel_ts) / MCLK_IN_MHZ * 1e-6f;
+  else
+    dt = (RELOAD_CYCLES - Vel_ts + mt6701_Vel->Last_Vel_ts) / MCLK_IN_MHZ *
+         1e-6f;
 
-  if (dt < 0.0001) {
+  if (dt < 0.0001)
     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;
-  Vel_Last_Angle = Vel_Angle;
+  mt6701_Vel->Last_Vel_ts = Vel_ts;
+  mt6701_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 mt6701_Vel->velocity;
 }

3rd/mt6701.h

@@ -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 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_get_angle_degree(void);
-float GetAngle(void);
-float GetAngle_NoTrack(void);
-float GetVelocity(void);
+float GetAngle(struct MT6701 *mt6701);
+float GetAngle_NoTrack(struct MT6701 *mt6701);
+float GetVelocity(struct MT6701 *mt6701_Vel);
 
 #ifdef __cplusplus
 }

3rd/pid_control.c

@@ -1,51 +1,42 @@
+#include "pid_control.h"
+#include "config.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) / MCLK_IN_MHZ * 1e-6;
+  else
+    Ts = (0xFFFFFF - Timestamp + pid->Timestamp_Last) / MCLK_IN_MHZ * 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;
+}

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

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

3rd/pwm.h

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

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

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

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 = 50.0; // 串口目标值
+float M1_Target = 50.0; // 串口目标值
 
 #define UART_PACKET_SIZE (6)
 
@@ -108,25 +114,17 @@ 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); // 速度模式
+    Print_Velocity(0);
 
-    // 开环
-    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();
+      M0_Target = Target;
     }
   }
 }
@@ -135,8 +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);
-    // printf("%s",gUartRxPacket);
+    // printf("Target is %f \n", Target);
+    //  printf("%s",gUartRxPacket);
     DL_GPIO_togglePins(LED_PORT, LED_PA0_PIN);
 
     break;