add config.h ; try to OpenLoop control, not passed

3rd/config.h

@@ -0,0 +1,7 @@
+#ifndef CONFIG_H
+#define CONFIG_H
+
+#define DEBUG_ENABLED true
+#define DEBUG_MT_ENABLED false
+#define DEBUG_DFOC true
+#endif /* ti_msp_dl_config_h */

3rd/dfoc.c

@@ -1,11 +1,14 @@
 
 #include <stdlib.h>
+#include <stdio.h>
 #include "pwm.h"
 #include "mt6701.h"
 #include "delay.h"
 #include "lowpass_filter.h"
 #include "pid_control.h"
 #include <math.h>
+#include <ti_msp_dl_config.h>
+#include "config.h"
 
 #define PI         3.14159265359f
 #define _3PI_2     4.71238898f
@@ -20,6 +23,8 @@ float zero_electric_Angle = 0.0;
 extern int pp;
 extern int Dir;
 
+
+
 void Motor_en()
 {
     //	GPIO_InitTypeDef GPIO_InitStructure;
@@ -66,9 +71,16 @@ void SetPwm(float Ua, float Ub, float Uc)
     dc_b = constrain(U_b / voltage_power_supply, 0.0f, 1.0f);
     dc_c = constrain(U_c / voltage_power_supply, 0.0f, 1.0f);
 
-    PWM_Channel1(dc_a * 4800.0f);  // 频率15k
-    PWM_Channel2(dc_b * 4800.0f);
-    PWM_Channel3(dc_c * 4800.0f);
+    PWM_Channel1(dc_a * 1000.0f);  // 频率15k
+    PWM_Channel2(dc_b * 1000.0f);
+    PWM_Channel3(dc_c * 1000.0f);
+    if(DEBUG_ENABLED & DEBUG_DFOC)
+    {
+        printf("dc_a %f \n", dc_a);
+        printf("dc_b %f \n", dc_b);
+        printf("dc_c %f \n", dc_c);
+    }
+
 
 }
 
@@ -120,7 +132,40 @@ void Set_Angle(float Target)
     SetPhaseVoltage(Uq, 0, electricAngle());
 }
 
-
+double shaft_angle;
+double openloop_timestamp;
+float velocityopenloop(float target)
+{
+    float Uq = 0.0;
+    double Ts = 0.0;
+
+    uint32_t now_ts  =   DL_SYSTICK_getValue();
+
+
+    if(now_ts < openloop_timestamp)
+    {
+        Ts = (openloop_timestamp - now_ts) / 32.0f * 1e-6f;
+    }
+    else
+    {
+        Ts = (0xFFFFFF - now_ts + openloop_timestamp) / 32.0f * 1e-6f;
+    }
+
+    if(Ts < 0 || Ts >= 0.005)
+    {
+        Ts = 0.001f;
+    }
+
+    shaft_angle = normalizeAngle(shaft_angle + pp * target * Ts);
+
+    Uq = voltage_limit;
+
+    SetPhaseVoltage(Uq, 0, shaft_angle);
+
+    openloop_timestamp = now_ts;
+
+    return Uq;
+}
 
 
 

3rd/dfoc.h

@@ -11,6 +11,7 @@ void FOC_Init(float power);
 float electricAngle(void);
 float GetCommand(void);
 void Set_Angle(float Target);
+float velocityopenloop(float target);
 
 
 

3rd/mt6701.c

@@ -2,10 +2,14 @@
 #include "ti_msp_dl_config.h"
 #include "uart_redircet.h"
 #include "stdio.h"
+#include "config.h"
+
 volatile int16_t angle;
 volatile float angle_f;
 volatile float angle_f_rad;
 
+volatile bool gIsI2cError = false;
+
 /* Data sent to the Target */
 uint8_t gTxPacket[I2C_TX_PACKET_SIZE] =
 {
@@ -13,7 +17,7 @@ uint8_t gTxPacket[I2C_TX_PACKET_SIZE] =
 };
 
 /* Data received from Target */
-volatile uint8_t gRxPacket[I2C_RX_PACKET_SIZE];
+volatile uint8_t gRxPacket[I2C_RX_PACKET_SIZE] = {0};
 
 /* I2C clock configuration */
 DL_I2C_ClockConfig gI2CclockConfig;
@@ -46,8 +50,13 @@ void MT6701_iic_read_angel(void)
      * gDelayCycles = 3 I2C functional clock cycles
      * gDelayCycles = 3 * I2C clock divider * (CPU clock freq / I2C clock freq)
      */
-    gDelayCycles = (3 * (gI2CclockConfig.divideRatio + 1)) *
+    gDelayCycles = (5 * (gI2CclockConfig.divideRatio + 1)) *
                    (CPUCLK_FREQ / gClockSelFreq);
+    if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
+    {
+        printf("i2c before writing -------\n");
+
+    }
 
     /*
      * Fill FIFO with data. This example will send a MAX of 8 bytes since it
@@ -66,6 +75,11 @@ void MT6701_iic_read_angel(void)
     DL_I2C_startControllerTransfer(I2C_1_INST, I2C_TARGET_ADDRESS,
                                    DL_I2C_CONTROLLER_DIRECTION_TX, I2C_TX_PACKET_SIZE);
 
+    if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
+    {
+        printf("i2c writing done -------\n");
+    }
+
     /* Workaround for errata I2C_ERR_13 */
     delay_cycles(gDelayCycles);
 
@@ -78,7 +92,12 @@ void MT6701_iic_read_angel(void)
     if(DL_I2C_getControllerStatus(I2C_1_INST) &
             DL_I2C_CONTROLLER_STATUS_ERROR)
     {
-        printf("i2c error --------------------------------------------------------------------------------\n");
+        gIsI2cError = true;
+
+        if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
+        {
+            printf("i2c error ------------------------------------------\n");
+        }
         /* LED will remain high if there is an error */
         __BKPT(0);
         return;
@@ -92,6 +111,11 @@ void MT6701_iic_read_angel(void)
     /* Add delay between transfers */
     delay_cycles(1000);
 
+    if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
+    {
+        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);
@@ -106,6 +130,13 @@ void MT6701_iic_read_angel(void)
             ;
         gRxPacket[i] = DL_I2C_receiveControllerData(I2C_1_INST);
     }
+
+    if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
+    {
+        printf("i2c reading done -------\n");
+    }
+
+
 }
 volatile float Last_ts = 0.0;
 volatile float last_angle = 0.0;