一、基本硬件准备
1. 带编码器的直流电机
- 电机参数对比
- 编码器参数对比
我直接选用的是光电编码器的电机,霍尔传感器的精度低一些,对于后面需要做位置控制的需求,我选择了带光电编码器的电机。
2. 直流电机驱动器
可选的直流电机驱动器有很多,我这里选择了TB6612FNG电机驱动模块,主要原因是它无需散热片,体积小,最主要的是他在切换电机运行方向的时候,不需要切换控制器PWM波输出引脚,只需要对芯片的IO进行控制即可反向运行,非常方便。(资料下载:提取码: 6ejw)
- 芯片参数
- 逻辑部分输入电压VCC:3.3~5V
- 驱动部分输入电压VM:2.5~12V
- 驱动电机路数:2通道
- 单通道最大连续驱动电流:1.2A
- 启动峰值:2A/3.2A(连续脉冲/单脉冲)
- 接口方式:2.54mm间距排针
- 模块尺寸:20 × 19.5(mm)
- 引脚功能定义
- DIR1:电机M1的方向控制引脚
- PWM2:电机M2的速度控制引脚
- DIR2:电机M2的方向控制引脚
- GND:逻辑部分电源负极
- VCC:逻辑部分电源正极
- M1+:M1路电机输出1
- M1-:M1路电机输出2
- M2+:M2路电机输出1
- M2-:M2路电机输出2
- GND:电机电源负极
- VM(<=12V):电机电源正极
二、STM32微控制器相关驱动代码准备
1.按键处理基本代码(主要用于PWM脉宽输出修改)
按下按键一下,增加CCR1_Val+10比较器值,更新至PWM脉冲定时器比较寄存器中,从而修改输出PWM波的脉宽,进而决定了驱动器提供给点击的电功率。
key.c
1 #include <stdio.h> 2 #include <string.h> 3 #include "stm32f4xx.h" 4 #include "stm32f4xx_gpio.h" 5 #include "stm32f4xx_rcc.h" 6 #include "key.h" 7 8 void KEY_GPIO_Config(void) 9 { 10 GPIO_InitTypeDef GPIO_InitStructure; 11 12 RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE); 13 14 GPIO_InitStructure.GPIO_Pin=GPIO_Pin_0; 15 GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IN; 16 GPIO_InitStructure.GPIO_OType=GPIO_OType_OD; 17 GPIO_InitStructure.GPIO_Speed=GPIO_Speed_100MHz; 18 GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_NOPULL; 19 GPIO_Init(GPIOA,&GPIO_InitStructure); 20 } 21 22 uint8_t KEY_Read_Val(void) 23 { 24 return GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0); 25 }
key.h
#ifndef __KEY_H #define __KEY_H #include "stm32f4xx.h" void KEY_GPIO_Config(void); uint8_t KEY_Read_Val(void); #endif
2.PWM脉宽方波信号输出(主要用于控制电机的运行功率)
PWM脉宽弱点信号通过调制大功率电机驱动芯片TB6612F,从而输出12V@1.2A的PWM脉冲信号,根据驱动器输出脉宽从而可以确定输出的电功率为12V*1.2A*pulse%,例如当前脉冲宽度为10%,则输出平均功率为1.44W。
pwm_output.c
#include "stm32f4xx.h" #include "stm32f4xx_gpio.h" #include "stm32f4xx_rcc.h" #include "stm32f4xx_tim.h" #include "pwm_output.h" uint32_t CCR1_Val = 333; uint32_t CCR2_Val = 249; uint32_t CCR3_Val = 166; uint32_t CCR4_Val = 83; /** * @brief Configure the TIM3 Output Channels. * @param None * @retval None */ static void PWM_TIM_Config(void) { GPIO_InitTypeDef GPIO_InitStructure; /* TIM3 clock enable */ RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); /* GPIOC clock enable */ RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); /* GPIOC Configuration: TIM3 CH1 (PC6), TIM3 CH2 (PC7), TIM3 CH3 (PC8) and TIM3 CH4 (PC9) */ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ; GPIO_Init(GPIOC, &GPIO_InitStructure); /* Connect TIM3 pins to AF2 */ GPIO_PinAFConfig(GPIOC, GPIO_PinSource6, GPIO_AF_TIM3); GPIO_PinAFConfig(GPIOC, GPIO_PinSource7, GPIO_AF_TIM3); GPIO_PinAFConfig(GPIOC, GPIO_PinSource8, GPIO_AF_TIM3); GPIO_PinAFConfig(GPIOC, GPIO_PinSource9, GPIO_AF_TIM3); } void TIM_PWM_Initial(void) { uint16_t PrescalerValue = 0; TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; PWM_TIM_Config(); /* ----------------------------------------------------------------------- TIM3 Configuration: generate 4 PWM signals with 4 different duty cycles. In this example TIM3 input clock (TIM3CLK) is set to 2 * APB1 clock (PCLK1), since APB1 prescaler is different from 1. TIM3CLK = 2 * PCLK1 PCLK1 = HCLK / 4 => TIM3CLK = HCLK / 2 = SystemCoreClock /2 To get TIM3 counter clock at 21 MHz, the prescaler is computed as follows: Prescaler = (TIM3CLK / TIM3 counter clock) - 1 Prescaler = ((SystemCoreClock /2) /21 MHz) - 1 To get TIM3 output clock at 30 KHz, the period (ARR)) is computed as follows: ARR = (TIM3 counter clock / TIM3 output clock) - 1 = 665 TIM3 Channel1 duty cycle = (TIM3_CCR1/ TIM3_ARR)* 100 = 50% TIM3 Channel2 duty cycle = (TIM3_CCR2/ TIM3_ARR)* 100 = 37.5% TIM3 Channel3 duty cycle = (TIM3_CCR3/ TIM3_ARR)* 100 = 25% TIM3 Channel4 duty cycle = (TIM3_CCR4/ TIM3_ARR)* 100 = 12.5% Note: SystemCoreClock variable holds HCLK frequency and is defined in system_stm32f4xx.c file. Each time the core clock (HCLK) changes, user had to call SystemCoreClockUpdate() function to update SystemCoreClock variable value. Otherwise, any configuration based on this variable will be incorrect. ----------------------------------------------------------------------- */ /* Compute the prescaler value */ PrescalerValue = (uint16_t) ((SystemCoreClock /2) / 21000000) - 1; /* Time base configuration */ TIM_TimeBaseStructure.TIM_Period = 665; TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue; TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); /* PWM1 Mode configuration: Channel1 */ TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_Pulse = CCR1_Val; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; TIM_OC1Init(TIM3, &TIM_OCInitStructure); TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable); /* PWM1 Mode configuration: Channel2 */ TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_Pulse = CCR2_Val; TIM_OC2Init(TIM3, &TIM_OCInitStructure); TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable); /* PWM1 Mode configuration: Channel3 */ TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_Pulse = CCR3_Val; TIM_OC3Init(TIM3, &TIM_OCInitStructure); TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable); /* PWM1 Mode configuration: Channel4 */ TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_Pulse = CCR4_Val; TIM_OC4Init(TIM3, &TIM_OCInitStructure); TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable); TIM_ARRPreloadConfig(TIM3, ENABLE); /* TIM3 enable counter */ TIM_Cmd(TIM3, ENABLE); }
pwm_output.h
#ifndef __PWM_OUTPUT_H #define __PWM_OUTPUT_H void TIM_PWM_Initial(void); #endif
3. TIM定时器外部脉冲信号捕获
外部定时器主要用来捕获来自编码器的脉冲输出信号,通过编码器输出脉冲信号计数结果以及定时器的采样周期从而计算出当前光电编码器输出脉冲信号的频率,进而通过电机传动减速比计算出受控轴的转速。
cap_input.c
1 #include "stm32f4xx.h" 2 #include "stm32f4xx_gpio.h" 3 #include "stm32f4xx_rcc.h" 4 #include "stm32f4xx_tim.h" 5 #include "cap_input.h" 6 7 /* Private function prototypes -----------------------------------------------*/ 8 static void CAP_TIM_Config(void) 9 { 10 GPIO_InitTypeDef GPIO_InitStructure; 11 NVIC_InitTypeDef NVIC_InitStructure; 12 13 /* TIM1 clock enable */ 14 RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); 15 16 /* GPIOA clock enable */ 17 RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE); 18 19 /* TIM1 channel 2 pin (PE.11) configuration */ 20 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11; 21 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; 22 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; 23 GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; 24 GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; 25 GPIO_Init(GPIOE, &GPIO_InitStructure); 26 27 /* Connect TIM pins to AF2 */ 28 GPIO_PinAFConfig(GPIOE, GPIO_PinSource11, GPIO_AF_TIM1); 29 30 /* Enable the TIM1 global Interrupt */ 31 NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn; 32 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; 33 NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; 34 NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; 35 NVIC_Init(&NVIC_InitStructure); 36 } 37 38 void TIM_CAP_Initial(void) 39 { 40 TIM_ICInitTypeDef TIM_ICInitStructure; 41 42 CAP_TIM_Config(); 43 44 /* TIM1 configuration: Input Capture mode --------------------- 45 The external signal is connected to TIM1 CH2 pin (PE.11) 46 The Rising edge is used as active edge, 47 The TIM1 CCR2 is used to compute the frequency value 48 ------------------------------------------------------------ */ 49 50 TIM_ICInitStructure.TIM_Channel = TIM_Channel_2; 51 TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; 52 TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; 53 TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; 54 TIM_ICInitStructure.TIM_ICFilter = 0x0; 55 56 TIM_ICInit(TIM1, &TIM_ICInitStructure); // TIM1_CC_IRQHandler 57 58 /* TIM enable counter */ 59 TIM_Cmd(TIM1, ENABLE); 60 61 /* Enable the CC2 Interrupt Request */ 62 TIM_ITConfig(TIM1, TIM_IT_CC2, ENABLE); 63 }
cap_input.h
#ifndef __CAP_INPUT_H #define __CAP_INPUT_H void TIM_CAP_Initial(void); #endif
4. USART串口数据输出
串口主要用于采样结果的打印,以及一些其他数据信息的展示,方便用来调试相关的程序代码.
usart.c
1 #include <stdio.h> 2 #include <string.h> 3 #include "stm32f4xx_usart.h" 4 #include "stm32f4xx_gpio.h" 5 #include "stm32f4xx_rcc.h" 6 #include "stm32f4xx_usart.h" 7 #include "usart.h" 8 9 typedef enum 10 { 11 COM1 = 0, 12 COM2 = 1 13 } COM_TypeDef; 14 15 #define COMn 1 16 17 /** 18 * @brief Definition for COM port1, connected to USART3 19 */ 20 #define EVAL_COM1 USART3 21 #define EVAL_COM1_CLK RCC_APB1Periph_USART3 22 #define EVAL_COM1_TX_PIN GPIO_Pin_10 23 #define EVAL_COM1_TX_GPIO_PORT GPIOC 24 #define EVAL_COM1_TX_GPIO_CLK RCC_AHB1Periph_GPIOC 25 #define EVAL_COM1_TX_SOURCE GPIO_PinSource10 26 #define EVAL_COM1_TX_AF GPIO_AF_USART3 27 #define EVAL_COM1_RX_PIN GPIO_Pin_11 28 #define EVAL_COM1_RX_GPIO_PORT GPIOC 29 #define EVAL_COM1_RX_GPIO_CLK RCC_AHB1Periph_GPIOC 30 #define EVAL_COM1_RX_SOURCE GPIO_PinSource11 31 #define EVAL_COM1_RX_AF GPIO_AF_USART3 32 #define EVAL_COM1_IRQn USART3_IRQn 33 34 USART_TypeDef* COM_USART[COMn] = {EVAL_COM1}; 35 GPIO_TypeDef* COM_TX_PORT[COMn] = {EVAL_COM1_TX_GPIO_PORT}; 36 GPIO_TypeDef* COM_RX_PORT[COMn] = {EVAL_COM1_RX_GPIO_PORT}; 37 38 const uint32_t COM_USART_CLK[COMn] = {EVAL_COM1_CLK}; 39 const uint32_t COM_TX_PORT_CLK[COMn] = {EVAL_COM1_TX_GPIO_CLK}; 40 const uint32_t COM_RX_PORT_CLK[COMn] = {EVAL_COM1_RX_GPIO_CLK}; 41 const uint16_t COM_TX_PIN[COMn] = {EVAL_COM1_TX_PIN}; 42 const uint16_t COM_RX_PIN[COMn] = {EVAL_COM1_RX_PIN}; 43 const uint16_t COM_TX_PIN_SOURCE[COMn] = {EVAL_COM1_TX_SOURCE}; 44 const uint16_t COM_RX_PIN_SOURCE[COMn] = {EVAL_COM1_RX_SOURCE}; 45 const uint16_t COM_TX_AF[COMn] = {EVAL_COM1_TX_AF}; 46 const uint16_t COM_RX_AF[COMn] = {EVAL_COM1_RX_AF}; 47 48 /** 49 * @brief Configures COM port. 50 * @param COM: Specifies the COM port to be configured. 51 * This parameter can be one of following parameters: 52 * @arg COM1 53 * @arg COM2 54 * @param USART_InitStruct: pointer to a USART_InitTypeDef structure that 55 * contains the configuration information for the specified USART peripheral. 56 * @retval None 57 */ 58 void STM_EVAL_COMInit(COM_TypeDef COM, USART_InitTypeDef* USART_InitStruct) 59 { 60 GPIO_InitTypeDef GPIO_InitStructure; 61 USART_ClockInitTypeDef USART_ClockInitStructure; 62 63 /* Enable GPIO clock */ 64 RCC_AHB1PeriphClockCmd(COM_TX_PORT_CLK[COM] | COM_RX_PORT_CLK[COM], ENABLE); 65 66 if (COM == COM1) 67 { 68 /* Enable UART clock */ 69 RCC_APB1PeriphClockCmd(COM_USART_CLK[COM], ENABLE); 70 } 71 72 /* Connect PXx to USARTx_Tx*/ 73 GPIO_PinAFConfig(COM_TX_PORT[COM], COM_TX_PIN_SOURCE[COM], COM_TX_AF[COM]); 74 75 /* Connect PXx to USARTx_Rx*/ 76 GPIO_PinAFConfig(COM_RX_PORT[COM], COM_RX_PIN_SOURCE[COM], COM_RX_AF[COM]); 77 78 /* Configure USART Tx as alternate function */ 79 GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; 80 GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; 81 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; 82 83 GPIO_InitStructure.GPIO_Pin = COM_TX_PIN[COM]; 84 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 85 GPIO_Init(COM_TX_PORT[COM], &GPIO_InitStructure); 86 87 /* Configure USART Rx as alternate function */ 88 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; 89 GPIO_InitStructure.GPIO_Pin = COM_RX_PIN[COM]; 90 GPIO_Init(COM_RX_PORT[COM], &GPIO_InitStructure); 91 92 USART_ClockInitStructure.USART_Clock = USART_Clock_Disable; 93 USART_ClockInitStructure.USART_CPOL = USART_CPOL_Low; 94 USART_ClockInitStructure.USART_CPHA = USART_CPHA_2Edge; 95 USART_ClockInitStructure.USART_LastBit = USART_LastBit_Disable; 96 97 USART_ClockInit(COM_USART[COM], &USART_ClockInitStructure); 98 99 /* USART configuration */ 100 USART_Init(COM_USART[COM], USART_InitStruct); 101 102 /* Enable USART */ 103 USART_Cmd(COM_USART[COM], ENABLE); 104 } 105 106 /** 107 * @brief Configures the USART Peripheral. 108 * @param None 109 * @retval None 110 */ 111 void USART_Config(void) 112 { 113 USART_InitTypeDef USART_InitStructure; 114 115 /* USARTx configured as follows: 116 - BaudRate = 115200 baud 117 - Word Length = 8 Bits 118 - One Stop Bit 119 - No parity 120 - Hardware flow control disabled (RTS and CTS signals) 121 - Receive and transmit enabled 122 */ 123 USART_InitStructure.USART_BaudRate = 115200; // 38400 124 USART_InitStructure.USART_WordLength = USART_WordLength_8b; 125 USART_InitStructure.USART_StopBits = USART_StopBits_1; 126 USART_InitStructure.USART_Parity = USART_Parity_No; 127 USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; 128 USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; 129 130 STM_EVAL_COMInit(COM1, &USART_InitStructure); 131 } 132 133 void u32tostr(unsigned long dat,char *str) 134 { 135 char temp[20]; 136 unsigned char i=0,j=0; 137 i=0; 138 while(dat) 139 { 140 temp[i]=dat%10+0x30; 141 i++; 142 dat/=10; 143 } 144 j=i; 145 for(i=0;i<j;i++) 146 { 147 str[i]=temp[j-i-1]; 148 } 149 if(!i) {str[i++]='0';} 150 str[i]=0; 151 } 152 153 unsigned long strtou32(char *str) 154 { 155 unsigned long temp=0; 156 unsigned long fact=1; 157 unsigned char len=strlen(str); 158 unsigned char i; 159 for(i=len;i>0;i--) 160 { 161 temp+=((str[i-1]-0x30)*fact); 162 fact*=10; 163 } 164 return temp; 165 } 166 167 void USART3_Putc(unsigned char c) 168 { 169 USART3->DR = (u8)c; //??????????????? 170 while((USART3->SR&0X40)==0); //?????? 171 } 172 173 void USART3_Puts(char * str) 174 { 175 while(*str) 176 { 177 USART3->DR= *str++; 178 while((USART3->SR&0X40)==0);//?????? 179 } 180 } 181 182 void UART_Send_Enter(void) 183 { 184 USART3_Putc(0x0d); 185 USART3_Putc(0x0a); 186 } 187 188 void UART_Send_Str(char *s) 189 { 190 191 for(;*s;s++) 192 { 193 if(*s=='\n') 194 UART_Send_Enter(); 195 else 196 USART3_Putc(*s); 197 } 198 } 199 200 void UART_Put_Num(unsigned long dat) 201 { 202 char temp[20]; 203 u32tostr(dat,temp); 204 UART_Send_Str(temp); 205 } 206 207 void UART_Put_Inf(char *inf,unsigned long dat) 208 { 209 UART_Send_Str(inf); 210 UART_Put_Num(dat); 211 UART_Send_Str("\n"); 212 }
usart.h
1 #ifndef __USART_H 2 #define __USART_H 3 4 void UART_Send_Enter(void); 5 6 void UART_Send_Str(char *s); 7 8 void UART_Put_Num(unsigned long dat); 9 void UART_Put_Inf(char *inf,unsigned long dat); 10 11 void u32tostr(unsigned long dat,char *str); 12 unsigned long strtou32(char *str) ; 13 void USART3_Puts( char * str); 14 void USART3_Putc(unsigned char c); 15 16 void USART_Config(void); 17 18 #endif
示波器测试结果如下所示,启动黄色为电机编码器反馈的脉冲信号,通过测量得到的脉冲频率为9.542KHz,而与之对应的当前PWM波频率为10KHz,Duty=12.6%.
完整的工程代码参考这里。
三、实验数据采集+系统输入输出关系分析
1. 数据采集结果
数据采集主程序代码(main.c):
1 #include <stdio.h> 2 #include <string.h> 3 #include "stm32f4xx.h" 4 #include "stm32f4xx_gpio.h" 5 #include "stm32f4xx_rcc.h" 6 #include "stm32f4xx_usart.h" 7 #include "pwm_output.h" 8 #include "cap_input.h" 9 #include "usart.h" 10 #include "key.h" 11 12 extern uint32_t uwTIM1Freq; 13 extern uint32_t CCR1_Val; 14 extern uint32_t CCR2_Val; 15 extern uint32_t CCR3_Val; 16 extern uint32_t CCR4_Val; 17 18 void delay_ms(unsigned int t) 19 { 20 unsigned int i; 21 for(i=0;i<10000;i++){ 22 while(t>0){ 23 t--; 24 } 25 } 26 } 27 28 void LED_GPIO_Config(void) 29 { 30 GPIO_InitTypeDef GPIO_InitStructure; 31 32 RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD,ENABLE); 33 34 GPIO_InitStructure.GPIO_Pin=GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15; 35 GPIO_InitStructure.GPIO_Mode=GPIO_Mode_OUT; 36 GPIO_InitStructure.GPIO_OType=GPIO_OType_PP; 37 GPIO_InitStructure.GPIO_Speed=GPIO_Speed_100MHz; 38 GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_NOPULL; 39 GPIO_Init(GPIOD,&GPIO_InitStructure); 40 } 41 42 int main(void) 43 { 44 uint32_t CCR_val = CCR4_Val; 45 unsigned long long fre_sum = 0; 46 unsigned long long fre_avg = 0; 47 int count = 0; 48 SystemInit(); 49 USART_Config(); 50 TIM_PWM_Initial(); 51 TIM_CAP_Initial(); 52 KEY_GPIO_Config(); 53 LED_GPIO_Config(); 54 while(1) 55 { 56 if(1 == KEY_Read_Val()){ 57 while(1 == KEY_Read_Val()); 58 CCR1_Val += 10; 59 if(665 < CCR1_Val) CCR1_Val = 0; 60 CCR2_Val += 10; 61 if(665 < CCR2_Val) CCR2_Val = 0; 62 CCR3_Val += 10; 63 if(665 < CCR3_Val) CCR3_Val = 0; 64 CCR4_Val += 10; 65 if(665 < CCR4_Val) CCR4_Val = 0; // PC9 66 TIM_PWM_Initial(); 67 } 68 GPIO_ResetBits(GPIOD, GPIO_Pin_12); 69 GPIO_ResetBits(GPIOD, GPIO_Pin_13); 70 GPIO_ResetBits(GPIOD, GPIO_Pin_14); 71 GPIO_ResetBits(GPIOD, GPIO_Pin_15); 72 if(count<10) 73 { 74 fre_sum += uwTIM1Freq; 75 count++; 76 } 77 if(count==10 && CCR_val != CCR4_Val) 78 { 79 fre_avg = fre_sum/10; 80 fre_sum = 0; 81 count = 0; 82 CCR_val = CCR4_Val; 83 // UART_Send_Str("Frequence:"); 84 UART_Put_Num(fre_avg); 85 // UART_Send_Str(" CCR1:"); 86 UART_Send_Str(" "); 87 UART_Put_Num(CCR4_Val); 88 // UART_Send_Str(" PWM1:"); 89 UART_Send_Str(" "); 90 UART_Put_Num(CCR4_Val*100/663); 91 UART_Send_Enter(); 92 } 93 delay_ms(100000); 94 delay_ms(100000); 95 // delay_ms(100000); 96 // delay_ms(100000); 97 GPIO_SetBits(GPIOD, GPIO_Pin_12); 98 GPIO_SetBits(GPIOD, GPIO_Pin_13); 99 GPIO_SetBits(GPIOD, GPIO_Pin_14); 100 GPIO_SetBits(GPIOD, GPIO_Pin_15); 101 delay_ms(100000); 102 delay_ms(100000); 103 // delay_ms(100000); 104 // delay_ms(100000); 105 } 106 }
MATLAB可视化结果如下(数据下载):
1 close all 2 % fd = load("CurveDataSet.txt"); 3 A = importdata('UsartSampleData.txt'); 4 % plot(fd(:,1),fd(:,2)) 5 plot(A(:,3),A(:,1))
可以从上述数据结果中看出,PWM脉冲宽度与电机的转速呈现为线性关系,故此使用PID控制器控制PWM-Speed的控制系统是非常合适的。
2. 数据拟合结果
可以从上述直线拟合结果中看出,当前的直线方程为 y=953*x-4923,直线吻合的非常好,但是考虑当输入PWM脉宽为0时,物理上的电路输出功率为0,而方程得出了反向的转速,这是不合理的,应该需要对后面的区段进行分段设计分析。
四、PID Controller控制器基本源代码
1.PID_Controller.c
1 #include <stdio.h> 2 #include <math.h> 3 #include <time.h> 4 #include <stdlib.h> 5 #include "PIDController.h" 6 7 /********************************************************************** 8 * 函数名称: // S_PID * PID_Controller_Initial(double Kp, double Ki, double Kd, double ReferVal) 9 10 * 功能描述: // PID控制系统初始化函数,输入PID控制器的Kp、Ki、Kd控制参数,输出增量控制值 11 * 访问的表: // 12 * 修改的表: // 13 * 输入参数: // double Kp: PID 比例控制因子 14 * 输入参数: // double Ki: PID 积分控制因子 15 * 输入参数: // double Kd: PID 微分控制因子 16 * 输入参数: // double ReferVal: PID控制器目标值 17 18 * 输出参数: // 19 * 返 回 值: // S_PID *: PID控制器基本参数结构体 20 * 其它说明: // 其它说明 21 * 修改日期 修改人 修改内容 22 * ----------------------------------------------- 23 * 2021/11/02 XXXX XXXX 24 ***********************************************************************/ 25 S_PID * PID_Controller_Initial(double Kp, double Ki, double Kd, double ReferVal) 26 { 27 S_PID * pt_pid = (S_PID *)malloc(sizeof(S_PID)); 28 pt_pid->Kp = Kp; 29 pt_pid->Ki = Ki; 30 pt_pid->Kd = Kd; 31 pt_pid->ReferVal = ReferVal; 32 pt_pid->ErrorLast = 0; 33 pt_pid->ErrorPrev = 0; 34 pt_pid->ErrorSum = 0; 35 return pt_pid; 36 } 37 38 /********************************************************************** 39 * 函数名称: // S_PID * PID_Controller_Initial_40E(void) 40 41 * 功能描述: // 40E差速控制 PID控制系统初始化函数,输入PID控制器的Kp、Ki、Kd控制参数,输出增量控制值 42 * 访问的表: // 43 * 修改的表: // 44 * 输入参数: // 45 46 * 输出参数: // 47 * 返 回 值: // S_PID *: PID控制器基本参数结构体 48 * 其它说明: // 其它说明 49 * 修改日期 修改人 修改内容 50 * ----------------------------------------------- 51 * 2021/11/02 XXE_XX XXXX 52 ***********************************************************************/ 53 S_PID * PID_Controller_Initial_40E(void) 54 { 55 56 return PID_Controller_Initial(0.03, 0.3, 0.05, 0); 57 } 58 59 /********************************************************************** 60 * 函数名称: // double PID_Controller_Increa(S_PID *pt_pid, double dF) 61 62 * 功能描述: // 运动初始化函数,根据默认曲线特征参数计算四个起始控制点,初始化系统状态信息 63 * 访问的表: // 64 * 修改的表: // 65 * 输入参数: // S_PID *pt_pid:系统PID参数初始化Kp、Ki、Kd参数 66 * 输入参数: // double dF:采样系统采集的左右轮受力差 67 68 * 输出参数: // 69 * 返 回 值: // double : PID控制器增量输出 70 * 其它说明: // 其它说明 71 * 修改日期 修改人 修改内容 72 * ----------------------------------------------- 73 * 2021/11/02 XXXX XXXX 74 ***********************************************************************/ 75 double PID_Controller_Increa(S_PID *pt_pid, double dF) 76 { 77 double error; 78 double IcrVal; 79 error = pt_pid->ReferVal - dF; // 当前误差计算 80 IcrVal = pt_pid->Kp * (error - pt_pid->ErrorLast) + pt_pid->Ki * error + pt_pid->Kd * (error - 2*pt_pid->ErrorLast + pt_pid->ErrorPrev); 81 pt_pid->ErrorPrev = pt_pid->ErrorLast; // 前一次误差更新 82 pt_pid->ErrorLast = error; // 前两次误差更新 83 return IcrVal; 84 } 85 86 /********************************************************************** 87 * 函数名称: // int PID_Controller_Destroy(S_PID *pt_pid) 88 89 * 功能描述: // PID系统资源回收函数,将PID相关参数结构体等销毁回收 90 * 访问的表: // 91 * 修改的表: // 92 * 输入参数: // S_PID *pt_pid:系统PID参数初始化Kp、Ki、Kd参数 93 94 * 输出参数: // 95 * 返 回 值: // int : 0 成功 -1 失败 96 * 其它说明: // 其它说明 97 * 修改日期 修改人 修改内容 98 * ----------------------------------------------- 99 * 2021/11/02 XXXX XXXX 100 ***********************************************************************/ 101 int PID_Controller_Destroy(S_PID *pt_pid) 102 { 103 if(NULL == pt_pid) 104 { 105 return -1; 106 } 107 else 108 { 109 free(pt_pid); 110 return 0; 111 } 112 } 113 114 /********************************************************************** 115 * 函数名称: // double SystemFunc(double SysInput) 116 117 * 功能描述: // 系统响应函数,主要用来模拟输入数据得到受控系统的输出值,同时在系统输出上增加适当随机系统扰动来模拟真是的受控系统状态 118 * 访问的表: // 119 * 修改的表: // 120 * 输入参数: // double SysInput:系统输入 121 122 * 输出参数: // 123 * 返 回 值: // double :系统输出 124 * 其它说明: // 其它说明 125 * 修改日期 修改人 修改内容 126 * ----------------------------------------------- 127 * 2021/11/02 XXXX XXXX 128 ***********************************************************************/ 129 double SystemFunc(double SysInput) 130 { 131 double SysOutput; 132 double RandVal; 133 srand(time(0)); // 随机种子生成 134 RandVal = (double)((rand()%10) - 5) / 5.0; // 随机系统输出扰动 135 SysOutput = 953 * SysInput - 4923 + RandVal; // 线性系统输出结构计算 136 return SysOutput; 137 }
2.PID_Controller.h
1 #ifndef _PID_CONTROLLER_H_ 2 #define _PID_CONTROLLER_H_ 3 4 typedef struct PID{ 5 double Kp; // PID 比例控制因子 6 double Ki; // PID 积分控制因子 7 double Kd; // PID 微分控制因子 8 double ReferVal; // PID控制器目标值 9 double ErrorLast; // 上一次系统误差 10 double ErrorPrev; // 上上次系统误差 11 double ErrorSum; // 系统误差累计 12 }S_PID; 13 14 /********************************************************************** 15 * 函数名称: // S_PID * PID_Controller_Initial(double Kp, double Ki, double Kd, double ReferVal) 16 17 * 功能描述: // 运动初始化函数,根据默认曲线特征参数计算四个起始控制点,初始化系统状态信息 18 * 访问的表: // 19 * 修改的表: // 20 * 输入参数: // double Kp: PID 比例控制因子 21 * 输入参数: // double Ki: PID 积分控制因子 22 * 输入参数: // double Kd: PID 微分控制因子 23 * 输入参数: // double ReferVal: PID控制器目标值 24 25 * 输出参数: // 26 * 返 回 值: // S_PID *: PID控制器基本参数结构体 27 * 其它说明: // 其它说明 28 * 修改日期 修改人 修改内容 29 * ----------------------------------------------- 30 * 2021/11/02 XXXX XXXX 31 ***********************************************************************/ 32 S_PID * PID_Controller_Initial(double Kp, double Ki, double Kd, double ReferVal); 33 34 /********************************************************************** 35 * 函数名称: // double PID_Controller_Increa(S_PID *pt_pid, double dF) 36 37 * 功能描述: // 增量式PID控制器输入输出值算法 38 * 访问的表: // 39 * 修改的表: // 40 * 输入参数: // S_PID *pt_pid:系统PID参数初始化Kp、Ki、Kd参数 41 * 输入参数: // double dF:采样系统采集的左右轮受力差 42 43 * 输出参数: // 44 * 返 回 值: // double : PID控制器增量输出 45 * 其它说明: // 其它说明 46 * 修改日期 修改人 修改内容 47 * ----------------------------------------------- 48 * 2021/11/02 XXXX XXXX 49 ***********************************************************************/ 50 double PID_Controller_Increa(S_PID *pt_pid, double dF); 51 52 /********************************************************************** 53 * 函数名称: // int PID_Controller_Destroy(S_PID *pt_pid) 54 55 * 功能描述: // PID系统资源回收函数,将PID相关参数结构体等销毁回收 56 * 访问的表: // 57 * 修改的表: // 58 * 输入参数: // S_PID *pt_pid:系统PID参数初始化Kp、Ki、Kd参数 59 60 * 输出参数: // 61 * 返 回 值: // int : 0 成功 -1 失败 62 * 其它说明: // 其它说明 63 * 修改日期 修改人 修改内容 64 * ----------------------------------------------- 65 * 2021/11/02 XXXX XXXX 66 ***********************************************************************/ 67 int PID_Controller_Destroy(S_PID *pt_pid); 68 69 /********************************************************************** 70 * 函数名称: // double SystemFunc(double SysInput) 71 72 * 功能描述: // 系统响应函数,主要用来模拟输入数据得到受控系统的输出值,同时在系统输出上增加适当随机系统扰动来模拟真是的受控系统状态 73 * 访问的表: // 74 * 修改的表: // 75 * 输入参数: // double SysInput:系统输入 76 77 * 输出参数: // 78 * 返 回 值: // double :系统输出 79 * 其它说明: // 其它说明 80 * 修改日期 修改人 修改内容 81 * ----------------------------------------------- 82 * 2021/11/02 XXXX XXXX 83 ***********************************************************************/ 84 double SystemFunc(double SysInput); 85 86 #endif
3. main.c
1 #include <stdio.h> 2 #include <math.h> 3 #include <time.h> 4 #include "PIDController.h" 5 6 int main(void) 7 { 8 double StandardVal = -10000; 9 double InitInput = 0; // System Default Initial Input Value. 10 double SysOutput; 11 int time_step = 0; 12 double IcrSum = 0; 13 S_PID * pt_pid = PID_Controller_Initial(0.0001, 0.0001, 0.0001, StandardVal); // 0.03, 0.3, 0.05 | 0.01, 0.1, 0.05, 0 double Kp, double Ki, double Kd, double ReferVal 14 for(time_step=0; time_step<900; time_step++) // Totally Sample times: 100. 15 { 16 SysOutput = SystemFunc(InitInput); // This should be Replaced by the Physical Value Sampled by the Sensor. 17 // printf("SysOutput:%f InitInput:%f\n", SysOutput, InitInput); // Show out the System Output. 18 printf("%f %f %f\n",SysOutput, StandardVal - SysOutput, InitInput); // Show out the System Output. 19 IcrSum += PID_Controller_Increa(pt_pid, SysOutput); // This PID Controller will control the Speed-Increasement for the Left&Right wheel. 20 InitInput += IcrSum; // Inrease the DeltaSpeedValue. 21 if(time_step == 300){ 22 StandardVal = 20000; 23 PID_Controller_Destroy(pt_pid); 24 pt_pid = PID_Controller_Initial(0.0001, 0.0001, 0.0001, StandardVal); 25 } 26 if(time_step == 600){ 27 StandardVal = 60000; 28 PID_Controller_Destroy(pt_pid); 29 pt_pid = PID_Controller_Initial(0.0001, 0.0001, 0.0001, StandardVal); 30 } 31 } 32 PID_Controller_Destroy(pt_pid); 33 return 0; 34 }
4. PIDDataViwer.m
1 close all 2 A = importdata("PIDDataSet.txt"); 3 figure, 4 plot(A(:,1),'r') % System Output 5 hold on, 6 plot(A(:,2),'b') % System Error 7 hold on, 8 plot(A(:,3),'g') % System Input 9 figure, 10 plot(A(:,3),'g') % System Input
控制结果如下所示(红色-系统输出 & 蓝色-系统误差 & 绿色-系统输入):
通过上述Matlab可视化结果可以发现,PID控制系统大概在100次采样周期内将对应的速度控制在目标速度位置处,想继续优化PID控制器的调节时间,可以通过修改PID控制参数来完成,由于当前采用的是增量式的PID控制算法,在MATLAB中无法进行仿真优化测试,在下一章节介绍位置式PID控制算法时将会介绍MATLAB工具箱进行参数整定和调节。
七、参数部署+实际调参
实际调试结果如下图所示,10000脉冲的跨度,PID控制电机完成时间为5s左右(轮子等加速过程需要时间,因此是延迟系统),调试过程问题如下:
1. 调试过程问题
调试过程中出现一顿一顿的情况,通过观察串口输出情况,发现编码器计算的频率值有一个错误的尖峰值影响了PID控制系统,解决办法:采用一维滤波器对采样值进行中值滤波,同时取消多次同时捕获同一个值的情况,从而避免了这种错误采样值的出现,具体代码节选如下:
1 if(count<FilterSize && uwTIM1Freq != MiddleFilterArray[count]) 2 { 3 MiddleFilterArray[count] = uwTIM1Freq; 4 count++; 5 } 6 if(count == FilterSize) 7 { 8 middle_filter(MiddleFilterArray, FilterSize); 9 fre_sum = FilterSize/2+FilterSize%2; 10 count = 0; 11 12 UART_Send_Str("Frequence:"); 13 UART_Put_Num(MiddleFilterArray[fre_sum]); 14 UART_Send_Str(" CCR4:"); 15 UART_Put_Num(CCR4_Val); 16 UART_Send_Str(" ST:"); 17 UART_Put_Num(standard); 18 UART_Send_Str(" PWM1:"); 19 UART_Put_Num(CCR4_Val*100/663); 20 UART_Send_Enter(); 21 22 IcrSum += PID_Controller_Increa(pt_pid, MiddleFilterArray[fre_sum]); // This PID Controller will control the Speed-Increasement for the Left&Right wheel. 23 if(IcrSum > 5){IcrSum = 5;} 24 if(IcrSum < -5){IcrSum = -5;} 25 InitInput += IcrSum; 26 27 if(InitInput > 663){ 28 InitInput = 663; 29 } 30 if(InitInput < 80){ 31 InitInput = 80; 32 } 33 CCR4_Val = (uint32_t)InitInput; 34 TIM_PWM_Initial(); 35 }
2. 参数部署以及MATLAB仿真可视化结果
PID参数为:[0.0001, 0.000001, 0.00001]
可以看到系统在50次采样之后趋于稳定的标准输出,然而可以预见的是,对于有一定延迟的电机控制系统,其响应时间应该和电机&电源&驱动器&负载等各个方面的参数相关,因此实际的控制响应速度还需要实际测量。
3. 实际情况下调参输出结果如下