move to ccs based on 39cc590260f6d69837dd16740b7b8612e8935592

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_ENABLED false
+#endif

3rd/delay.c

@@ -0,0 +1,9 @@
+#include "delay.h"
+#include "ti_msp_dl_config.h"
+void delay_ms(uint16_t ms)
+{
+    while(ms--)
+    {
+        delay_cycles(CPUCLK_FREQ / 1000);
+    }
+}

3rd/delay.h

@@ -0,0 +1,7 @@
+#ifndef delay_h
+#define delay_h
+#include <stdint.h>
+
+void delay_ms(uint16_t ms);
+
+#endif /* ti_msp_dl_config_h */

3rd/dfoc.c

@@ -0,0 +1,186 @@
+
+#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
+#define _1_SQRT3 	 0.57735026919f
+#define _2_SQRT3   1.15470053838f
+
+volatile float Ua = 0, Ub = 0, Uc = 0, Ualpha, Ubeta = 0, dc_a = 0, dc_b = 0, dc_c = 0;
+const float voltage_limit = 8;
+const float voltage_power_supply = 12.0f;
+volatile float zero_electric_Angle = 0.0;
+
+extern int pp;
+extern int Dir;
+
+
+
+void Motor_en()
+{
+    //	GPIO_InitTypeDef GPIO_InitStructure;
+    //
+    //	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
+    //
+    //	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+    //	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
+    //	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+    //	GPIO_Init(GPIOA,&GPIO_InitStructure);
+    //
+    //	GPIO_SetBits(GPIOA, GPIO_Pin_8);
+}
+
+//限制幅值
+float constrain(float amt, float low, float high)
+{
+    return ((amt < low) ? (low) : ((amt) > (high) ? (high) : (amt)));
+}
+
+//将角度归化到0-2PI
+float normalizeAngle(float angle)
+{
+    volatile float a = fmod(angle, 2 * PI);
+    return ((a >= 0) ? a : (a + 2 * PI));
+}
+
+float electricAngle(void)
+{
+    return normalizeAngle((GetAngle_NoTrack() * pp * Dir) - zero_electric_Angle);
+}
+
+void SetPwm(float Ua, float Ub, float Uc)
+{
+    volatile float U_a = 0.0;
+    volatile float U_b = 0.0;
+    volatile float U_c = 0.0;
+
+    U_a = constrain(Ua, 0.0f, voltage_limit);
+    U_b = constrain(Ub, 0.0f, voltage_limit);
+    U_c = constrain(Uc, 0.0f, voltage_limit);
+    //printf("Ua : %f  -- Ub : %f -- Uc : %f -- U_a : %f  -- U_b : %f -- U_c : %f \n",Ua,Ub,Uc,U_a,U_b,U_c);
+    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);
+
+    PWM_Channel1(dc_a * 1000.0f);  // 频率15k
+    PWM_Channel2(dc_b * 1000.0f);
+    PWM_Channel3(dc_c * 1000.0f);
+
+
+
+}
+
+//FOC核心算法，克拉克逆变换/帕克逆变换
+volatile float test_angle = 0.0;
+volatile float last_test_angle = 0.0;
+void SetPhaseVoltage(float Uq, float Ud, 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);
+
+    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;
+
+    SetPwm(Ua, Ub, Uc);
+
+
+
+    last_test_angle = angle_el;
+}
+
+void Check_Sensor(void)
+{
+    //SetPhaseVoltage(3, 0, _3PI_2);
+    //delay_ms(3000);
+    zero_electric_Angle = electricAngle();
+    // SetPhaseVoltage(0, 0, _3PI_2);
+    //delay_ms(500);
+}
+
+void FOC_Init(float power)
+{
+    //voltage_power_supply = power;
+    //PWM_Init();
+    //CurrSense_Init();
+    //AS5600_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);
+    }
+    volatile float Uq = PID_Controller(0.067, 0.01, 0, (Target - Dir * langle) * 180 / PI);
+    //volatile float Uq = PID_Controller(0.06, 0, 0, (Target - Dir * langle) * 180 / PI);
+    if(DEBUG_ENABLED& DEBUG_DFOC_ENABLED)
+    {
+        printf("Uq is %f \n", Uq);
+    }
+    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 -- target : %f -- Ts : %f  \n",shaft_angle,target,Ts);
+    }
+
+
+    Uq = voltage_limit;
+
+    SetPhaseVoltage(Uq, 0, shaft_angle);
+    printf("shaft_angle : %f \n", shaft_angle);
+    openloop_timestamp = now_ts;
+
+    return Uq;
+}
+
+
+
+
+
+

3rd/dfoc.h

@@ -0,0 +1,20 @@
+#ifndef __DFOC_H
+#define __DFOC_H
+
+void Motor_en(void);
+float constrain(float amt, float low, float high);
+void SetPwm(float Ua, float Ub, float Uc);
+float normalizeAngle(float angle);
+void SetPhaseVoltage(float Uq, float Ud, float angle_el);
+void Check_Sensor(void);
+void FOC_Init(float power);
+float electricAngle(void);
+float GetCommand(void);
+void Set_Angle(float Target);
+float velocityopenloop(float target);
+
+
+
+#endif
+
+

3rd/lowpass_filter.c

@@ -0,0 +1,48 @@
+
+#include <stdint.h>
+#include <ti_msp_dl_config.h>
+
+float y = 0;
+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;
+
+    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;
+    }
+
+    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;
+
+    Last_y = y;
+    Last_Timesamp = Timesamp;
+
+
+    return y;
+}
+

3rd/lowpass_filter.h

@@ -0,0 +1,12 @@
+#ifndef __LOWPASS_FILTER_H
+#define __LOWPASS_FILTER_H
+
+
+float Lowpassfilter(float Tf, float x);
+float Lowpassfilter_sim(float x);
+
+
+
+#endif
+
+

3rd/mt6701.c

@@ -0,0 +1,215 @@
+#include "mt6701.h"
+#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;
+
+#define DEBUG_I2C	false
+
+/* Data sent to the Target */
+uint8_t gTxPacket[I2C_TX_PACKET_SIZE] =
+{
+    0x03
+};
+
+/* Data received from Target */
+volatile uint8_t gRxPacket[I2C_RX_PACKET_SIZE] = {0};
+
+/* I2C clock configuration */
+DL_I2C_ClockConfig gI2CclockConfig;
+/* Frequency of selected I2C clock*/
+volatile uint32_t gClockSelFreq;
+
+/* Cycles to delay after controller transfer initiated */
+volatile uint32_t gDelayCycles;
+
+/* I2C Target address */
+#define I2C_TARGET_ADDRESS (0x06)
+
+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)
+{
+    MT6701_iic_read_angel();
+    angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
+    angle_f_rad = (float)angle * _2PI / 16384;
+    if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
+    {
+        printf("angle_rad read back is %f \n", angle_f_rad);
+    }
+    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;
+
+    if(fabs(D_Angle_rad) > (0.8f * 2 * PI))
+    {
+        full_rotations = full_rotations + ((D_Angle_rad > 0) ? -1 : 1);
+    }
+
+    Last_Angle_rad = Angle_rad;
+
+    return (full_rotations * 2 * PI + Last_Angle_rad);
+}
+
+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;
+    }
+
+    if(dt < 0.0001)
+    {
+        dt = 10000;
+    }
+
+    float Vel_Angle = GetAngle();
+
+    float dv = Vel_Angle - Vel_Last_Angle;
+
+    float velocity = (Vel_Angle - Vel_Last_Angle) / dt;
+
+    Last_Vel_ts = Vel_ts;
+    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;
+}

3rd/mt6701.h

@@ -0,0 +1,32 @@
+#ifndef MT6701_H
+#define MT6701_H
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#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)
+void MT6701_iic_read_angel(void);
+void MT6701_get_angle_degree(void);
+float GetAngle(void);
+float GetAngle_NoTrack(void);
+float GetVelocity(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* MT6701_H */

3rd/pid_control.c

@@ -0,0 +1,51 @@
+#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)));
+}
+
+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;
+    }
+
+    if(Ts <= 0 || Ts > 0.05f)
+    {
+        Ts = 0.001;
+    }
+
+    float proportion = Kp * Error;//P环
+
+    float intergration = Last_intergration + Ki * 0.5f * Ts * Error;//I环
+    intergration = _constrain(intergration, -limit, limit);
+
+    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;
+}
+

3rd/pid_control.h

@@ -0,0 +1,9 @@
+#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);
+
+#endif
+
+

3rd/pwm.c

@@ -0,0 +1,25 @@
+#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 PWM_Channel2(uint16_t 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);
+}
+
+
+
+
+
+
+
+

3rd/pwm.h

@@ -0,0 +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);
+
+#endif
+
+

3rd/uart_redircet.c

@@ -0,0 +1,53 @@
+#include "ti_msp_dl_config.h"
+#include <stdio.h>
+#include <string.h>
+
+
+/* ARM Compiler 6 半主机模式禁用声明 */
+#if (__ARMCC_VERSION >= 6010050)
+    __asm(".global __use_no_semihosting\n\t");
+    __asm(".global __ARM_use_no_argv \n\t");
+#else
+    #pragma import(__use_no_semihosting)
+#endif
+
+FILE __stdout;
+
+/* 系统退出函数实现 */
+void _sys_exit(int x)
+{
+    x = x;
+    while(1)
+        ; /* 死循环防止程序异常退出 */
+}
+
+/* ARM Compiler 6 printf重定向实现 */
+int fputc(int c, FILE *stream)
+{
+    /* 等待UART发送缓冲区就绪 */
+    while(DL_UART_Main_isBusy(UART_0_INST))
+        ;
+
+    /* 发送字符 */
+    DL_UART_Main_transmitData(UART_0_INST, (uint8_t)c);
+    return c;
+}
+
+int fputs(const char *restrict s, FILE *restrict stream)
+{
+    uint16_t i, len;
+    len = strlen(s);
+    for(i = 0; i < len; i++)
+    {
+        fputc(s[i], stream);
+    }
+    return len;
+}
+
+int puts(const char *_ptr)
+{
+    int count = fputs(_ptr, stdout);
+    count += fputc('\n', stdout); /* 自动添加换行 */
+    return count;
+}
+

3rd/uart_redircet.h

@@ -0,0 +1,7 @@
+#ifndef Uart_h
+#define Uart_h
+#include <stdint.h>
+
+void UART_Console_write(const uint8_t *data, uint16_t size);
+
+#endif /* ti_msp_dl_config_h */