pwm uart gipo i2c

3rd/dfoc.c

@@ -0,0 +1,129 @@
+
+#include <stdlib.h>
+#include "pwm.h"
+#include "mt6701.h"
+#include "delay.h"
+#include "lowpass_filter.h"
+#include "pid_control.h"
+#include <math.h>
+
+#define PI         3.14159265359f
+#define _3PI_2     4.71238898f
+#define _1_SQRT3 	 0.57735026919f
+#define _2_SQRT3   1.15470053838f
+
+float Ua=0,Ub=0,Uc=0,Ualpha,Ubeta=0,dc_a=0,dc_b=0,dc_c=0;
+float voltage_limit = 8;
+float voltage_power_supply = 0;
+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)
+{
+	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)
+{
+	float U_a=0.0;
+	float U_b=0.0;
+	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);
+	
+	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 * 4800.0f);  // 频率15k
+	PWM_Channel2(dc_b * 4800.0f);
+	PWM_Channel3(dc_c * 4800.0f);
+	
+}
+
+//FOC核心算法，克拉克逆变换/帕克逆变换
+float test_angle = 0.0;
+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)
+{
+	float angle = GetAngle();
+	float Uq = PID_Controller(0.133, 0.01, 0, (Target - Dir*angle)*180/PI);
+	SetPhaseVoltage(Uq, 0, electricAngle());
+}
+
+
+
+
+
+
+
+

3rd/dfoc.h

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

3rd/lowpass_filter.c

@@ -0,0 +1,42 @@
+
+#include <stdint.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;
+
+	//TODO
+	uint32_t Timesamp = 0;
+	//uint32_t Timestamp = SysTick->VAL;
+	if(Timesamp < Last_Timesamp) dt = (float)(Last_Timesamp - Timesamp)/9*1e-6;
+	else
+		dt = (float)(0xFFFFFF - Timesamp + 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;
+	
+	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

@@ -1,6 +1,7 @@
 #include "MT6701.h"
 #include "ti_msp_dl_config.h"
-
+#include "uart_redircet.h"
+#include "stdio.h"
 volatile int16_t angle;
 volatile float angle_f;
 volatile float angle_f_rad;
@@ -77,6 +78,7 @@ void MT6701_iic_read_angel(void)
     if(DL_I2C_getControllerStatus(I2C_1_INST) &
             DL_I2C_CONTROLLER_STATUS_ERROR)
     {
+			//printf("i2c error \n");
         /* LED will remain high if there is an error */
         __BKPT(0);
     }

3rd/pid_control.c

@@ -0,0 +1,45 @@
+#include <stdint.h>
+
+#define  limit            6.3
+#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;
+	//TODO
+	uint32_t Timestamp = 0;
+	//uint32_t Timestamp = SysTick->VAL;
+  if(Timestamp < Timestamp_Last) Ts = (float)(Timestamp_Last - Timestamp)/9*1e-6;
+	else
+		Ts = (0xFFFFFF - Timestamp + Timestamp_Last)/9*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
+
+

empty.c

@@ -47,6 +47,7 @@ int main(void)
 {
     SYSCFG_DL_init();
     NVIC_EnableIRQ(UART_0_INST_INT_IRQN);
+    DL_TimerA_startCounter(PWM_0_INST);
     while (1) {
 			DL_GPIO_togglePins(LED_PORT,LED_PA0_PIN);
 			//MT6701_get_angle_degree();

empty.syscfg

@@ -13,6 +13,8 @@ const GPIO   = scripting.addModule("/ti/driverlib/GPIO", {}, false);
 const GPIO1  = GPIO.addInstance();
 const I2C    = scripting.addModule("/ti/driverlib/I2C", {}, false);
 const I2C1   = I2C.addInstance();
+const PWM    = scripting.addModule("/ti/driverlib/PWM", {}, false);
+const PWM1   = PWM.addInstance();
 const SYSCTL = scripting.addModule("/ti/driverlib/SYSCTL");
 const UART   = scripting.addModule("/ti/driverlib/UART", {}, false);
 const UART1  = UART.addInstance();
@@ -47,6 +49,25 @@ I2C1.sclPinConfig.onlyInternalResistor = scripting.forceWrite(false);
 I2C1.sclPinConfig.passedPeripheralType = scripting.forceWrite("Digital");
 I2C1.sclPinConfig.$name                = "ti_driverlib_gpio_GPIOPinGeneric3";
 
+PWM1.$name                      = "PWM_0";
+PWM1.ccIndex                    = [0,1,2];
+PWM1.clockDivider               = 2;
+PWM1.timerCount                 = 2000;
+PWM1.pwmMode                    = "CENTER_ALIGN";
+PWM1.PWM_CHANNEL_0.$name        = "ti_driverlib_pwm_PWMTimerCC0";
+PWM1.PWM_CHANNEL_0.dutyCycle    = 25;
+PWM1.PWM_CHANNEL_1.$name        = "ti_driverlib_pwm_PWMTimerCC1";
+PWM1.PWM_CHANNEL_1.dutyCycle    = 50;
+PWM1.ccp0PinConfig.$name        = "ti_driverlib_gpio_GPIOPinGeneric4";
+PWM1.ccp1PinConfig.$name        = "ti_driverlib_gpio_GPIOPinGeneric5";
+PWM1.peripheral.$assign         = "TIMA0";
+PWM1.peripheral.ccp0Pin.$assign = "PA21";
+PWM1.peripheral.ccp1Pin.$assign = "PA22";
+PWM1.peripheral.ccp2Pin.$assign = "PB20";
+PWM1.PWM_CHANNEL_2.$name        = "ti_driverlib_pwm_PWMTimerCC2";
+PWM1.PWM_CHANNEL_2.dutyCycle    = 75;
+PWM1.ccp2PinConfig.$name        = "ti_driverlib_gpio_GPIOPinGeneric6";
+
 SYSCTL.forceDefaultClkConfig = true;
 SYSCTL.peripheral.$assign    = "SYSCTL";
 

ti_msp_dl_config.c

@@ -40,6 +40,8 @@
 
 #include "ti_msp_dl_config.h"
 
+DL_TimerA_backupConfig gPWM_0Backup;
+
 /*
  *  ======== SYSCFG_DL_init ========
  *  Perform any initialization needed before using any board APIs
@@ -50,23 +52,50 @@ SYSCONFIG_WEAK void SYSCFG_DL_init(void)
     SYSCFG_DL_GPIO_init();
     /* Module-Specific Initializations*/
     SYSCFG_DL_SYSCTL_init();
+    SYSCFG_DL_PWM_0_init();
     SYSCFG_DL_I2C_1_init();
     SYSCFG_DL_UART_0_init();
     SYSCFG_DL_DMA_init();
+    /* Ensure backup structures have no valid state */
+	gPWM_0Backup.backupRdy 	= false;
+
+
+}
+/*
+ * User should take care to save and restore register configuration in application.
+ * See Retention Configuration section for more details.
+ */
+SYSCONFIG_WEAK bool SYSCFG_DL_saveConfiguration(void)
+{
+    bool retStatus = true;
+
+	retStatus &= DL_TimerA_saveConfiguration(PWM_0_INST, &gPWM_0Backup);
+
+    return retStatus;
 }
 
 
+SYSCONFIG_WEAK bool SYSCFG_DL_restoreConfiguration(void)
+{
+    bool retStatus = true;
+
+	retStatus &= DL_TimerA_restoreConfiguration(PWM_0_INST, &gPWM_0Backup, false);
+
+    return retStatus;
+}
 
 SYSCONFIG_WEAK void SYSCFG_DL_initPower(void)
 {
     DL_GPIO_reset(GPIOA);
     DL_GPIO_reset(GPIOB);
+    DL_TimerA_reset(PWM_0_INST);
     DL_I2C_reset(I2C_1_INST);
     DL_UART_Main_reset(UART_0_INST);
 
 
     DL_GPIO_enablePower(GPIOA);
     DL_GPIO_enablePower(GPIOB);
+    DL_TimerA_enablePower(PWM_0_INST);
     DL_I2C_enablePower(I2C_1_INST);
     DL_UART_Main_enablePower(UART_0_INST);
 
@@ -76,6 +105,13 @@ SYSCONFIG_WEAK void SYSCFG_DL_initPower(void)
 SYSCONFIG_WEAK void SYSCFG_DL_GPIO_init(void)
 {
 
+    DL_GPIO_initPeripheralOutputFunction(GPIO_PWM_0_C0_IOMUX,GPIO_PWM_0_C0_IOMUX_FUNC);
+    DL_GPIO_enableOutput(GPIO_PWM_0_C0_PORT, GPIO_PWM_0_C0_PIN);
+    DL_GPIO_initPeripheralOutputFunction(GPIO_PWM_0_C1_IOMUX,GPIO_PWM_0_C1_IOMUX_FUNC);
+    DL_GPIO_enableOutput(GPIO_PWM_0_C1_PORT, GPIO_PWM_0_C1_PIN);
+    DL_GPIO_initPeripheralOutputFunction(GPIO_PWM_0_C2_IOMUX,GPIO_PWM_0_C2_IOMUX_FUNC);
+    DL_GPIO_enableOutput(GPIO_PWM_0_C2_PORT, GPIO_PWM_0_C2_PIN);
+
     DL_GPIO_initPeripheralInputFunctionFeatures(GPIO_I2C_1_IOMUX_SDA,
         GPIO_I2C_1_IOMUX_SDA_FUNC, DL_GPIO_INVERSION_DISABLE,
         DL_GPIO_RESISTOR_NONE, DL_GPIO_HYSTERESIS_DISABLE,
@@ -116,6 +152,66 @@ SYSCONFIG_WEAK void SYSCFG_DL_SYSCTL_init(void)
 }
 
 
+/*
+ * Timer clock configuration to be sourced by  / 2 (16000000 Hz)
+ * timerClkFreq = (timerClkSrc / (timerClkDivRatio * (timerClkPrescale + 1)))
+ *   16000000 Hz = 16000000 Hz / (2 * (0 + 1))
+ */
+static const DL_TimerA_ClockConfig gPWM_0ClockConfig = {
+    .clockSel = DL_TIMER_CLOCK_BUSCLK,
+    .divideRatio = DL_TIMER_CLOCK_DIVIDE_2,
+    .prescale = 0U
+};
+
+static const DL_TimerA_PWMConfig gPWM_0Config = {
+    .pwmMode = DL_TIMER_PWM_MODE_CENTER_ALIGN,
+    .period = 2000,
+    .isTimerWithFourCC = true,
+    .startTimer = DL_TIMER_STOP,
+};
+
+SYSCONFIG_WEAK void SYSCFG_DL_PWM_0_init(void) {
+
+    DL_TimerA_setClockConfig(
+        PWM_0_INST, (DL_TimerA_ClockConfig *) &gPWM_0ClockConfig);
+
+    DL_TimerA_initPWMMode(
+        PWM_0_INST, (DL_TimerA_PWMConfig *) &gPWM_0Config);
+
+    // Set Counter control to the smallest CC index being used
+    DL_TimerA_setCounterControl(PWM_0_INST,DL_TIMER_CZC_CCCTL0_ZCOND,DL_TIMER_CAC_CCCTL0_ACOND,DL_TIMER_CLC_CCCTL0_LCOND);
+
+    DL_TimerA_setCaptureCompareOutCtl(PWM_0_INST, DL_TIMER_CC_OCTL_INIT_VAL_LOW,
+		DL_TIMER_CC_OCTL_INV_OUT_DISABLED, DL_TIMER_CC_OCTL_SRC_FUNCVAL,
+		DL_TIMERA_CAPTURE_COMPARE_0_INDEX);
+
+    DL_TimerA_setCaptCompUpdateMethod(PWM_0_INST, DL_TIMER_CC_UPDATE_METHOD_IMMEDIATE, DL_TIMERA_CAPTURE_COMPARE_0_INDEX);
+    DL_TimerA_setCaptureCompareValue(PWM_0_INST, 750, DL_TIMER_CC_0_INDEX);
+
+    DL_TimerA_setCaptureCompareOutCtl(PWM_0_INST, DL_TIMER_CC_OCTL_INIT_VAL_LOW,
+		DL_TIMER_CC_OCTL_INV_OUT_DISABLED, DL_TIMER_CC_OCTL_SRC_FUNCVAL,
+		DL_TIMERA_CAPTURE_COMPARE_1_INDEX);
+
+    DL_TimerA_setCaptCompUpdateMethod(PWM_0_INST, DL_TIMER_CC_UPDATE_METHOD_IMMEDIATE, DL_TIMERA_CAPTURE_COMPARE_1_INDEX);
+    DL_TimerA_setCaptureCompareValue(PWM_0_INST, 500, DL_TIMER_CC_1_INDEX);
+
+    DL_TimerA_setCaptureCompareOutCtl(PWM_0_INST, DL_TIMER_CC_OCTL_INIT_VAL_LOW,
+		DL_TIMER_CC_OCTL_INV_OUT_DISABLED, DL_TIMER_CC_OCTL_SRC_FUNCVAL,
+		DL_TIMERA_CAPTURE_COMPARE_2_INDEX);
+
+    DL_TimerA_setCaptCompUpdateMethod(PWM_0_INST, DL_TIMER_CC_UPDATE_METHOD_IMMEDIATE, DL_TIMERA_CAPTURE_COMPARE_2_INDEX);
+    DL_TimerA_setCaptureCompareValue(PWM_0_INST, 250, DL_TIMER_CC_2_INDEX);
+
+    DL_TimerA_enableClock(PWM_0_INST);
+
+
+    
+    DL_TimerA_setCCPDirection(PWM_0_INST , DL_TIMER_CC0_OUTPUT | DL_TIMER_CC1_OUTPUT | DL_TIMER_CC2_OUTPUT );
+
+
+}
+
+
 static const DL_I2C_ClockConfig gI2C_1ClockConfig = {
     .clockSel = DL_I2C_CLOCK_BUSCLK,
     .divideRatio = DL_I2C_CLOCK_DIVIDE_1,

ti_msp_dl_config.h

@@ -77,6 +77,32 @@ extern "C" {
 
 
 
+/* Defines for PWM_0 */
+#define PWM_0_INST                                                         TIMA0
+#define PWM_0_INST_IRQHandler                                   TIMA0_IRQHandler
+#define PWM_0_INST_INT_IRQN                                     (TIMA0_INT_IRQn)
+#define PWM_0_INST_CLK_FREQ                                             16000000
+/* GPIO defines for channel 0 */
+#define GPIO_PWM_0_C0_PORT                                                 GPIOA
+#define GPIO_PWM_0_C0_PIN                                         DL_GPIO_PIN_21
+#define GPIO_PWM_0_C0_IOMUX                                      (IOMUX_PINCM46)
+#define GPIO_PWM_0_C0_IOMUX_FUNC                     IOMUX_PINCM46_PF_TIMA0_CCP0
+#define GPIO_PWM_0_C0_IDX                                    DL_TIMER_CC_0_INDEX
+/* GPIO defines for channel 1 */
+#define GPIO_PWM_0_C1_PORT                                                 GPIOA
+#define GPIO_PWM_0_C1_PIN                                         DL_GPIO_PIN_22
+#define GPIO_PWM_0_C1_IOMUX                                      (IOMUX_PINCM47)
+#define GPIO_PWM_0_C1_IOMUX_FUNC                     IOMUX_PINCM47_PF_TIMA0_CCP1
+#define GPIO_PWM_0_C1_IDX                                    DL_TIMER_CC_1_INDEX
+/* GPIO defines for channel 2 */
+#define GPIO_PWM_0_C2_PORT                                                 GPIOB
+#define GPIO_PWM_0_C2_PIN                                         DL_GPIO_PIN_20
+#define GPIO_PWM_0_C2_IOMUX                                      (IOMUX_PINCM48)
+#define GPIO_PWM_0_C2_IOMUX_FUNC                     IOMUX_PINCM48_PF_TIMA0_CCP2
+#define GPIO_PWM_0_C2_IDX                                    DL_TIMER_CC_2_INDEX
+
+
+
 
 /* Defines for I2C_1 */
 #define I2C_1_INST                                                          I2C1
@@ -133,6 +159,7 @@ void SYSCFG_DL_init(void);
 void SYSCFG_DL_initPower(void);
 void SYSCFG_DL_GPIO_init(void);
 void SYSCFG_DL_SYSCTL_init(void);
+void SYSCFG_DL_PWM_0_init(void);
 void SYSCFG_DL_I2C_1_init(void);
 void SYSCFG_DL_UART_0_init(void);
 void SYSCFG_DL_DMA_init(void);