STM32F407 倒立摆核心代码
时间: 2023-10-18 07:05:04 浏览: 86
以下是一个基于STM32F407的倒立摆的核心代码,仅供参考:
```c
#include "stm32f4xx.h"
#include "stm32f4xx_conf.h"
/* 定义时钟频率 */
#define SYS_CLK_FREQ 168000000
/* 定义PWM输出通道 */
#define PWM_TIM TIM4
#define PWM_TIM_CH TIM_OC1InitTypeDef TIM_OCInitStruct;
#define PWM_TIM_CH_GPIO GPIOB
#define PWM_TIM_CH_PIN GPIO_Pin_6
#define PWM_TIM_CH_SOURCE GPIO_PinSource6
#define PWM_TIM_CH_AF GPIO_AF_TIM4
/* 定义编码器输入通道 */
#define ENCODER_TIM TIM3
#define ENCODER_TIM_CH1 GPIO_Pin_6
#define ENCODER_TIM_CH2 GPIO_Pin_7
#define ENCODER_TIM_CH_GPIO GPIOC
#define ENCODER_TIM_CH_SOURCE GPIO_PinSource6
#define ENCODER_TIM_CH_AF GPIO_AF_TIM3
/* 定义控制参数 */
#define Kp 1.0f
#define Ki 0.0f
#define Kd 0.0f
/* 定义采样周期 */
#define T 0.001f
/* 定义PID控制器参数 */
float error = 0.0f, error_last = 0.0f, integral = 0.0f, derivative = 0.0f, output = 0.0f;
/* 定义编码器计数器 */
int32_t encoder_count = 0;
/* 初始化PWM输出 */
void init_pwm(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_OCInitTypeDef TIM_OCInitStruct;
GPIO_InitTypeDef GPIO_InitStruct;
/* 使能GPIOB时钟 */
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
/* 配置GPIOB.6为PWM输出引脚 */
GPIO_InitStruct.GPIO_Pin = PWM_TIM_CH_PIN;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStruct.GPIO_Speed = GPIO_High_Speed;
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(PWM_TIM_CH_GPIO, &GPIO_InitStruct);
/* 配置PWM定时器 */
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
TIM_TimeBaseInitStruct.TIM_Period = 1000;
TIM_TimeBaseInitStruct.TIM_Prescaler = (SYS_CLK_FREQ / 1000000) - 1;
TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(PWM_TIM, &TIM_TimeBaseInitStruct);
/* 配置PWM输出通道 */
TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStruct.TIM_Pulse = 0;
TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OC1Init(PWM_TIM, &TIM_OCInitStruct);
/* 配置PWM输出GPIO复用映射 */
GPIO_PinAFConfig(PWM_TIM_CH_GPIO, PWM_TIM_CH_SOURCE, PWM_TIM_CH_AF);
/* 启动PWM输出 */
TIM_Cmd(PWM_TIM, ENABLE);
}
/* 初始化编码器输入 */
void init_encoder(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
GPIO_InitTypeDef GPIO_InitStruct;
TIM_ICInitTypeDef TIM_ICInitStruct;
/* 使能GPIOC时钟 */
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
/* 配置GPIOC.6和GPIOC.7为编码器输入引脚 */
GPIO_InitStruct.GPIO_Pin = ENCODER_TIM_CH1 | ENCODER_TIM_CH2;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStruct.GPIO_Speed = GPIO_High_Speed;
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(ENCODER_TIM_CH_GPIO, &GPIO_InitStruct);
/* 配置编码器输入GPIO复用映射 */
GPIO_PinAFConfig(ENCODER_TIM_CH_GPIO, ENCODER_TIM_CH_SOURCE, ENCODER_TIM_CH_AF);
/* 配置编码器输入定时器 */
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
TIM_TimeBaseInitStruct.TIM_Period = 0xFFFF;
TIM_TimeBaseInitStruct.TIM_Prescaler = 0;
TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(ENCODER_TIM, &TIM_TimeBaseInitStruct);
/* 配置编码器输入捕获通道 */
TIM_ICInitStruct.TIM_Channel = TIM_Channel_1;
TIM_ICInitStruct.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStruct.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStruct.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStruct.TIM_ICFilter = 0;
TIM_ICInit(ENCODER_TIM, &TIM_ICInitStruct);
TIM_ICInitStruct.TIM_Channel = TIM_Channel_2;
TIM_ICInitStruct.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStruct.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStruct.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStruct.TIM_ICFilter = 0;
TIM_ICInit(ENCODER_TIM, &TIM_ICInitStruct);
/* 启动编码器输入 */
TIM_Cmd(ENCODER_TIM, ENABLE);
}
/* 初始化系统时钟 */
void init_sys_clock(void)
{
RCC_DeInit();
RCC_HSEConfig(RCC_HSE_ON);
while (RCC_WaitForHSEStartUp() != SUCCESS)
{
}
RCC_PLLConfig(RCC_PLLSource_HSE, 8, 336, 2, 7);
RCC_PLLCmd(ENABLE);
while (RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET)
{
}
FLASH_SetLatency(FLASH_Latency_5);
FLASH_PrefetchBufferCmd(ENABLE);
RCC_HCLKConfig(RCC_SYSCLK_Div1);
RCC_PCLK1Config(RCC_HCLK_Div4);
RCC_PCLK2Config(RCC_HCLK_Div2);
RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);
}
/* 获取编码器计数器的值 */
int32_t get_encoder_count(void)
{
return TIM_GetCounter(ENCODER_TIM);
}
/* PID控制器 */
void PID_control(void)
{
/* 获取当前角度值 */
float angle = get_encoder_count() * 360.0f / 4096.0f;
/* 计算误差 */
error = 0.0f - angle;
/* 计算积分项 */
integral += error * T;
/* 计算微分项 */
derivative = (error - error_last) / T;
/* 计算输出 */
output = Kp * error + Ki * integral + Kd * derivative;
/* 更新误差 */
error_last = error;
/* 输出PWM信号 */
TIM_SetCompare1(PWM_TIM, (uint32_t)(500 + output));
}
/* 主函数 */
int main(void)
{
/* 初始化系统时钟 */
init_sys_clock();
/* 初始化PWM输出 */
init_pwm();
/* 初始化编码器输入 */
init_encoder();
/* 循环执行PID控制 */
while (1)
{
PID_control();
delay(T);
}
return 0;
}
```
这段代码实现了一个基本的倒立摆控制器,通过编码器获取倒立摆的位置信息,通过PID控制器计算控制信号,并通过PWM输出控制电机的转速,使得倒立摆能够保持在竖直位置上方不倒下。需要注意的是,这仅是一个核心代码,具体实现过程需要根据具体的硬件和功能需求进行调整和完善。
相关推荐
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)