STM32爬坡寻迹小车（PID算法）

只看该作者 · 2023-2-28 17:28

2 TIM2、TIM3

功能：TIM2和TIM3分别用于小车左后轮和右后轮编码器的正交解码，其中通道一连接编码器A相，通道二连接编码器B相，将定时器配置为编码器模式后，可根据A、B相的脉冲计数，即可获取电机的旋转情况。

只看该作者 · 2023-2-28 17:29

2 TIM2、TIM3

功能：TIM2和TIM3分别用于小车左后轮和右后轮编码器的正交解码，其中通道一连接编码器A相，通道二连接编码器B相，将定时器配置为编码器模式后，可根据A、B相的脉冲计数，即可获取电机的旋转情况。

只看该作者 · 2023-2-28 17:29

配置步骤：

使能定时器及端口时钟，配置对应引脚为浮空输入；
初始化定时器参数，包含自动重装值，分频系数，计数方式等；
选择时钟源，配置为编码器模式；
使能TIMX在CCRX上的预装载寄存器；
使能TIMX在ARR上的预装载寄存器允许位；
开启定时器。

只看该作者 · 2023-2-28 17:29

定时器基本结构体中，将预分频值设置为0（不使用系统时钟，无需分频），重装载值设置为1040（一般为编码器线圈数 * 4，即电机转一圈所计的数，若编码器线圈数未知，可设为最大65535），计数方式为向上计数。

只看该作者 · 2023-2-28 17:29

将编码器参数设置为通道一、二都计数（即A、B相的计数），可更加精确地获取脉冲，同时可获取方向。

只看该作者 · 2023-2-28 17:29

3 TIM4

功能：TIM4仅用作一个基本的定时器，用于定时进入中断函数，获取TIM2、TIM3的脉冲数，并转换为对应的速度，调用PID算法，将转换得到的实际速度通过算法计算，改变PWM1的占空比，使其不断减小与期望速度的误差，稳定于期望速度。

只看该作者 · 2023-2-28 17:29

配置步骤：

RCC开启定时器时钟；
初始化定时器参数，包含自动重装值，分频系数，计数方式等；
配置输出中断控制，使能更新中断；
配置NVIC，打开定时器通道；
使能定时器。

只看该作者 · 2023-2-28 17:29

定时器基本结构体中，将预分频值设置为72，重装载值设置为5000，即每5ms触发一次中断，调用PID算法计算并调整速度，计数方式为向上计数。

中断配置结构体中，选择对应的中断通道（TIM4_IRQn）并使能，设置抢占优先级和响应优先级为1。

只看该作者 · 2023-2-28 17:30

实物图

只看该作者 · 2023-2-28 17:30

只看该作者 · 2023-2-28 17:30

软件部分代码
pwm.c:使用高级定时器输出pwm波

复制

void TIM1_PWM_Init(u16 per,u16 psc)

{

        /*使能TIM4时钟*/

        RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1,ENABLE);

        

        /*使能GPIO*/

        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);

        

        /*使能AFIO*/

        RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);

        

        /*配置GPIO*/

        GPIO_InitTypeDef GPIO_InitStructure;

        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;//复用

        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 |GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_11;

        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

        GPIO_Init(GPIOA,&GPIO_InitStructure);

        

        /*设置重映射*/

        //GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3,ENABLE);//部分重映射        

        

        /*初始化定时器参数*/

        TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;

        TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频为1分频

        TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;//选择计数模式为向上计数

        TIM_TimeBaseInitStructure.TIM_Period = per;//配置周期（ARR自动重装器的值）

        TIM_TimeBaseInitStructure.TIM_Prescaler = psc;//配置PSC预分频器的值

        TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;//重复计数器的值，高级计数器才需配置

        TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStructure);

        //TIM_ClearFlag(TIM4,TIM_FLAG_Update);//先清除标志位，避免刚初始化就进入中断

        

        /*初始化PWM参数*/

        TIM_OCInitTypeDef TIM_OCInitStructure;

        TIM_OCInitStructure.TIM_Pulse = 0;

        TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;

        TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset;

        TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;//选择PWM1模式

        TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;//输出极性：低电平有效

        TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;

        TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;//使能

        TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;

        TIM_OC1Init(TIM1,&TIM_OCInitStructure);

        TIM_OC2Init(TIM1,&TIM_OCInitStructure);

        TIM_OC3Init(TIM1,&TIM_OCInitStructure);

        TIM_OC4Init(TIM1,&TIM_OCInitStructure);

        

        /*使能TIMX在CCRX上的预装载寄存器*/

        TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable);

        TIM_OC2PreloadConfig(TIM1,TIM_OCPreload_Enable);

        TIM_OC3PreloadConfig(TIM1,TIM_OCPreload_Enable);

        TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable);

        

        TIM_CtrlPWMOutputs(TIM1,ENABLE);

        

        /*使能TIMX在ARR上的预装载寄存器允许位*/

        //TIM_ARRPreloadConfig(TIM4,ENABLE);

        

        /*开启定时器*/

        TIM_Cmd(TIM1,ENABLE);

}

只看该作者 · 2023-2-28 17:30

zjjm.c:使用定时器2、3

复制

#include "stm32f10x.h"                  // Device header

 

/*

        配置TIM2,TIM3的CH1、CH2作编码器的A、B相；

        电机转速不同，机械几何位移量转换成的脉冲也不同，使得单位时间内TIM计数值不同，可由此判断电机速度的变化

*/

 

void TIM2_Init(u16 per,u16 psc)

{

        /*开启通用定时器TIM2和GPIO时钟*/

        RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);

        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);

        

        GPIO_InitTypeDef GPIO_InitStructure;

        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;

        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;

        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

        GPIO_Init(GPIOA,&GPIO_InitStructure);

        

        /*配置时基单元*/

        TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;

        TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频为1分频

        TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;//选择计数模式为向上计数

        /*

        定时频率 = 72M / (PSC+1) / (ARR+1)

        */

        TIM_TimeBaseInitStructure.TIM_Period = per;//已知编码器线数：线数*4-1

        TIM_TimeBaseInitStructure.TIM_Prescaler = psc;

        TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;//重复计数器的值，高级计数器才需配置

        TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);

        

        /*选择时钟源，配置正交解码*/

        TIM_ICInitTypeDef TIM_ICInitStructure;  

    TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_BothEdge ,TIM_ICPolarity_BothEdge);//TI12:A、B项都计数

    TIM_ICStructInit(&TIM_ICInitStructure);//缺省输入

    TIM_ICInitStructure.TIM_ICFilter = 0;  //滤波器

    TIM_ICInit(TIM2, &TIM_ICInitStructure);

        

        TIM_SetCounter(TIM2,0X7FFF); //TIM4->CNT=0X7FFF（65536的一半）

        /*启动定时器*/

        TIM_Cmd(TIM2,ENABLE);

}

 

void TIM3_Init(u16 per,u16 psc)

{

        /*开启通用定时器TIM2和GPIO时钟*/

        RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);

        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);

        

        GPIO_InitTypeDef GPIO_InitStructure;

        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;

        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;

        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

        GPIO_Init(GPIOA,&GPIO_InitStructure);

        

        /*配置时基单元*/

        TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;

        TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频为1分频

        TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;//选择计数模式为向上计数

        /*

        定时频率 = 72M / (PSC+1) / (ARR+1)

        */

        TIM_TimeBaseInitStructure.TIM_Period = per;//已知编码器线数：线数*4-1

        TIM_TimeBaseInitStructure.TIM_Prescaler = psc;

 

        TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;//重复计数器的值，高级计数器才需配置

        TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitStructure);

        

        /*选择时钟源，配置正交解码*/

        TIM_ICInitTypeDef TIM_ICInitStructure;  

    TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_BothEdge ,TIM_ICPolarity_BothEdge);//TI12:A、B项都计数

    TIM_ICStructInit(&TIM_ICInitStructure);//缺省输入

    TIM_ICInitStructure.TIM_ICFilter = 0;  //滤波器

    TIM_ICInit(TIM3, &TIM_ICInitStructure);

        

        TIM_SetCounter(TIM3,0X7FFF); //TIM4->CNT=0X7FFF（65536的一半）

        /*启动定时器*/

        TIM_Cmd(TIM3,ENABLE);

}

只看该作者 · 2023-2-28 17:30

timer.c:使用定时器4产生溢出中断，通过cnt的值计算应输出的占空比大小（只使用后轮进行调速）

复制

#include "stm32f10x.h"                  // Device header

#include "Timer.h"

#include "stdlib.h"

/*

        配置定时中断流程：

                1.RCC开启时钟（定时器基准时钟和外设工作时钟）

                2.选择时基单元的时钟源（对于定时中断，选择内部时钟源）

                3.配置时基单元（包括PSC预分频器，ARR自动重装器，CNT计数器）

                4.配置输出中断控制，使能更新中断，允许更新中断输出到NVIC

                5.配置NVIC，打开定时器通道，并分配优先级

                6.运行控制，使能计数器

*/

 

void TIM4_Init(u16 per,u16 psc)

{

        /*1、开启通用定时器TIM4时钟*/

        RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);

        

        /*2、选择时钟源为内部时钟*/

        //TIM_InternalClockConfig(TIM4);//默认为内部时钟

        

        /*3、配置时基单元*/

        TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;

        TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频为1分频

        TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;//选择计数模式为向上计数

        /*

        定时频率 = 72M / (PSC+1) / (ARR+1)

        */

        TIM_TimeBaseInitStructure.TIM_Period = per;//配置周期（ARR自动重装器的值）

        TIM_TimeBaseInitStructure.TIM_Prescaler = psc;//配置PSC预分频器的值

        TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStructure);

        

        /*4、使能更新中断*/

        TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE);

        

        /*5、配置NVIC*/

        

        NVIC_InitTypeDef NVIC_InitStructure;

        NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;//选择中断通道

        NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;//开启中断通道

        NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;//抢占优先级

        NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;//响应优先级

        NVIC_Init(&NVIC_InitStructure);        

        

        /*6、启动定时器*/

        TIM_Cmd(TIM4,ENABLE);

}

 

float Kp=10;//

float Ki=2;//

float Kd= 0.2;

int target_left=0;

int target_right=0;

 

int err_now1=0;

int err_now2=0;

int err_last1=0;

int err_last2=0;

int err_last_last1=0;

int err_last_last2=0;

int spd_now1=0;

int spd_now2=0;

int jisuan1=0;

int jisuan2=0;

int out_left=0;

int out_right=0;

 

void Change_Target(int t_l,int t_r)

{

        target_left = t_l;

        target_right = t_r;

}

 

void TIM4_IRQHandler(void)

{

        if(TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET)

        {

                int cnt_left,cnt_right;

                cnt_left = abs((TIM2->CNT)-0X7FFF);

                cnt_right = abs((TIM3->CNT)-0X7FFF);

                

                TIM2->CNT = 0X7FFF;

                TIM3->CNT = 0X7FFF;

                

                //左电机        

                spd_now1 = cnt_left;

                err_now1 = target_left - spd_now1;

                jisuan1 = Kp*(err_now1-err_last1) + Ki*err_now1 + Kd*(err_now1+err_last_last1-2*err_last1);

                out_left += jisuan1;

                if(out_left<0)

                        out_left = 0;

                if(out_left>100) 

                        out_left = 100; 

                TIM_SetCompare1(TIM1,out_left);        

                TIM_SetCompare2(TIM1,out_left);        

                err_last_last1 = err_last1;

                err_last1 = err_now1;

                

                //右电机

                spd_now2 = cnt_right;

                err_now2 = target_right - spd_now2;

                jisuan2 = Kp*(err_now2-err_last2) + Ki*err_now2 + Kd*(err_now2+err_last_last2-2*err_last2);

                out_right += jisuan2;

                if(out_right<0)

                        out_right = 0;

                if(out_right>100) 

                        out_right = 100;         

                TIM_SetCompare3(TIM1,out_right);        

                TIM_SetCompare4(TIM1,out_right);

                err_last_last2 = err_last2;

                err_last2 = err_now2; 

                

                TIM_ClearITPendingBit(TIM4, TIM_IT_Update);

        }

 

}

只看该作者 · 2023-2-28 17:31

car.c

复制

#include "stm32f10x.h"                  // Device header

#include "pwm.h"

#include "Timer.h"

/*

 

TIM1(高级定时器)：CH1-CH4: PWM输出控制四个电机  

                PA8    左前

                PA9    左后

                PA10   右前

                PA11   右后

TIM2:CH1-CH2: 左后轮编码器A、B相   PA0,PA1

TIM3:CH1-CH2: 右后轮编码器A、B相   PA6,PA7

TIM4:定时测速调速 

        

        红外：PB12,PB13,PB14

 

TB6612N逻辑输入：（1 0正转，0 1反转）

        PB0,PB1:左前         通道1

        PB5,PB8:右前     通道3

        PA3,PA2:左后         通道2

        PB10,PB11:右后   通道4

*/

 

void GPIOB_Init(void)

{

        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);

        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);

        

        GPIO_InitTypeDef GPIO_InitStructure;

        /*红外out引脚：PB12,PB13,PB14*/

        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;//浮空输入

        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14;

        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

        GPIO_Init(GPIOB,&GPIO_InitStructure);

        

        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;//推挽输出

        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3;

        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

        GPIO_Init(GPIOA,&GPIO_InitStructure);

        

        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;//推挽输出

        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_5 | GPIO_Pin_8 | GPIO_Pin_10 | GPIO_Pin_11;

        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

        GPIO_Init(GPIOB,&GPIO_InitStructure);

        /*初始为低电平*/

        GPIO_ResetBits(GPIOB,GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_5 | GPIO_Pin_8 | GPIO_Pin_10 | GPIO_Pin_11);

        GPIO_ResetBits(GPIOA,GPIO_Pin_2 | GPIO_Pin_3);

}

 

 

void forward(u16 speed)

{

        TIM_SetCompare1(TIM1,speed);

        TIM_SetCompare2(TIM1,speed);

        TIM_SetCompare3(TIM1,speed);

        TIM_SetCompare4(TIM1,speed);

        GPIO_SetBits(GPIOB,GPIO_Pin_0| GPIO_Pin_5 | GPIO_Pin_10);

        GPIO_SetBits(GPIOA,GPIO_Pin_3);

        GPIO_ResetBits(GPIOB,GPIO_Pin_1| GPIO_Pin_8 | GPIO_Pin_11);

        GPIO_ResetBits(GPIOA,GPIO_Pin_2);

}

 

void back(u16 speed)

{

        TIM_SetCompare1(TIM1,speed);

        TIM_SetCompare2(TIM1,speed);

        TIM_SetCompare3(TIM1,speed);

        TIM_SetCompare4(TIM1,speed);

        GPIO_SetBits(GPIOB,GPIO_Pin_1| GPIO_Pin_8 | GPIO_Pin_11);

        GPIO_SetBits(GPIOA,GPIO_Pin_2);

        GPIO_ResetBits(GPIOB,GPIO_Pin_0 | GPIO_Pin_5 | GPIO_Pin_10);

        GPIO_ResetBits(GPIOA,GPIO_Pin_3);

}

 

void turn_left(u16 speed1,u16 speed2)

{

        TIM_SetCompare1(TIM1,speed1);

        TIM_SetCompare2(TIM1,speed1);

        TIM_SetCompare3(TIM1,speed2);

        TIM_SetCompare4(TIM1,speed2);

        GPIO_SetBits(GPIOB,GPIO_Pin_0| GPIO_Pin_5 | GPIO_Pin_10);

        GPIO_SetBits(GPIOA,GPIO_Pin_3);

        GPIO_ResetBits(GPIOB,GPIO_Pin_1| GPIO_Pin_8 | GPIO_Pin_11);

        GPIO_ResetBits(GPIOA,GPIO_Pin_2);

}

 

void turn_right(u16 speed1,u16 speed2)

{

        TIM_SetCompare1(TIM1,speed1);

        TIM_SetCompare2(TIM1,speed1);

        TIM_SetCompare3(TIM1,speed2);

        TIM_SetCompare4(TIM1,speed2);

        GPIO_SetBits(GPIOB,GPIO_Pin_0| GPIO_Pin_5 | GPIO_Pin_10);

        GPIO_SetBits(GPIOA,GPIO_Pin_3);

        GPIO_ResetBits(GPIOB,GPIO_Pin_1| GPIO_Pin_8 | GPIO_Pin_11);

        GPIO_ResetBits(GPIOA,GPIO_Pin_2);

}

void stop(void)

{

        GPIO_ResetBits(GPIOB,GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_5 | GPIO_Pin_8 | GPIO_Pin_10 | GPIO_Pin_11);

        GPIO_ResetBits(GPIOA,GPIO_Pin_2 | GPIO_Pin_3);

}

只看该作者 · 2024-5-6 08:03

整流器或功率开关管与每个电容端的连线长度和宽度都要一样

只看该作者 · 2024-5-6 09:06

不要根据不同的参数类型走不同的代码逻辑

只看该作者 · 2024-5-6 11:02

其他的信号和地要与这些高压引线和它的散热部分隔开

只看该作者 · 2024-5-6 12:05

对于没有else的场景，使用ifPresent即可

只看该作者 · 2024-5-6 13:08

每个Strategy交由Spring管理，并在构造后注册

只看该作者 · 2024-5-6 14:01

变压器隔离拓扑中，变压器有多少个绕组