基于stm32多功能自助刷墙机器人pid程序设计
时间: 2023-06-29 17:18:26 浏览: 59
以下是基于STM32的多功能自助刷墙机器人PID程序设计的示例代码:
```c
#include "stm32f10x.h"
#define MOTOR_LEFT_FORWARD GPIO_Pin_0 // 左电机正转引脚
#define MOTOR_LEFT_BACKWARD GPIO_Pin_1 // 左电机反转引脚
#define MOTOR_RIGHT_FORWARD GPIO_Pin_2 // 右电机正转引脚
#define MOTOR_RIGHT_BACKWARD GPIO_Pin_3 // 右电机反转引脚
#define ULTRASONIC_TRIG GPIO_Pin_0 // 超声波模块触发引脚
#define ULTRASONIC_ECHO GPIO_Pin_1 // 超声波模块接收引脚
float Kp = 0.5; // 比例系数
float Ki = 0.2; // 积分系数
float Kd = 0.1; // 微分系数
float SetSpeed = 100; // 目标速度
float ActualSpeed = 0; // 实际速度
float err = 0, err_last = 0; // 当前误差和上一次误差
float integral = 0; // 积分值
float differential = 0; // 微分值
void TIM2_Configuration() // 定时器2配置函数
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
TIM_DeInit(TIM2);
TIM_TimeBaseInitStructure.TIM_Period = 599; // 自动重装值
TIM_TimeBaseInitStructure.TIM_Prescaler = 7199; // 时钟预分频
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1; // 时钟分频
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up; // 向上计数模式
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStructure);
TIM_Cmd(TIM2, ENABLE);
}
void TIM3_Configuration() // 定时器3配置函数
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
TIM_DeInit(TIM3);
TIM_TimeBaseInitStructure.TIM_Period = 599; // 自动重装值
TIM_TimeBaseInitStructure.TIM_Prescaler = 7199; // 时钟预分频
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1; // 时钟分频
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up; // 向上计数模式
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseInitStructure);
TIM_Cmd(TIM3, ENABLE);
}
void TIM4_Configuration() // 定时器4配置函数
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
TIM_DeInit(TIM4);
TIM_TimeBaseInitStructure.TIM_Period = 599; // 自动重装值
TIM_TimeBaseInitStructure.TIM_Prescaler = 7199; // 时钟预分频
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1; // 时钟分频
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up; // 向上计数模式
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseInitStructure);
TIM_Cmd(TIM4, ENABLE);
}
void GPIO_Configuration() // GPIO配置函数
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB, ENABLE);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Pin = MOTOR_LEFT_FORWARD | MOTOR_LEFT_BACKWARD | MOTOR_RIGHT_FORWARD | MOTOR_RIGHT_BACKWARD;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Pin = ULTRASONIC_TRIG;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Pin = ULTRASONIC_ECHO;
GPIO_Init(GPIOB, &GPIO_InitStructure);
}
void NVIC_Configuration() // NVIC配置函数
{
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
void TIM2_IRQHandler() // 定时器2中断处理函数
{
TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
static unsigned char count = 0; // 计数器
static unsigned char flag = 0; // 标志位
static unsigned int current_distance = 0; // 当前距离
static unsigned int last_distance = 0; // 上一次距离
if(count == 0) // 每0.1秒进行一次距离测量
{
TIM_Cmd(TIM3, ENABLE);
GPIO_SetBits(GPIOB, ULTRASONIC_TRIG);
delay_us(10);
GPIO_ResetBits(GPIOB, ULTRASONIC_TRIG);
while(GPIO_ReadInputDataBit(GPIOB, ULTRASONIC_ECHO) == RESET);
TIM_SetCounter(TIM3, 0);
while(GPIO_ReadInputDataBit(GPIOB, ULTRASONIC_ECHO) == SET);
TIM_Cmd(TIM3, DISABLE);
current_distance = TIM_GetCounter(TIM3) * 17 / 1000;
count = 1;
}
if(count == 1) // 每0.1秒进行一次PID计算
{
flag = 1;
count = 0;
err = SetSpeed - ActualSpeed;
integral += err;
differential = err - err_last;
err_last = err;
ActualSpeed = Kp * err + Ki * integral + Kd * differential;
if(ActualSpeed > 0) // 向前行驶
{
GPIO_SetBits(GPIOA, MOTOR_LEFT_FORWARD);
GPIO_ResetBits(GPIOA, MOTOR_LEFT_BACKWARD);
GPIO_SetBits(GPIOA, MOTOR_RIGHT_FORWARD);
GPIO_ResetBits(GPIOA, MOTOR_RIGHT_BACKWARD);
}
else if(ActualSpeed < 0) // 向后行驶
{
GPIO_ResetBits(GPIOA, MOTOR_LEFT_FORWARD);
GPIO_SetBits(GPIOA, MOTOR_LEFT_BACKWARD);
GPIO_ResetBits(GPIOA, MOTOR_RIGHT_FORWARD);
GPIO_SetBits(GPIOA, MOTOR_RIGHT_BACKWARD);
}
else // 停止
{
GPIO_ResetBits(GPIOA, MOTOR_LEFT_FORWARD);
GPIO_ResetBits(GPIOA, MOTOR_LEFT_BACKWARD);
GPIO_ResetBits(GPIOA, MOTOR_RIGHT_FORWARD);
GPIO_ResetBits(GPIOA, MOTOR_RIGHT_BACKWARD);
}
}
}
int main(void)
{
TIM2_Configuration(); // 定时器2配置
TIM3_Configuration(); // 定时器3配置
TIM4_Configuration(); // 定时器4配置
GPIO_Configuration(); // GPIO配置
NVIC_Configuration(); // NVIC配置
while(1)
{
if(flag) // 如果计算出了新的速度,则更新目标速度
{
flag = 0;
last_distance = current_distance;
SetSpeed = last_distance;
}
}
}
```
这个程序使用超声波模块进行障碍物检测,每0.1秒进行一次距离测量并计算出当前距离。然后通过PID算法计算出当前速度,并将其转换为电机的工作状态。具体的细节可以根据实际情况进行调整。
相关推荐
![zip](https://img-home.csdnimg.cn/images/20210720083736.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)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)