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

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);

}        

#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);

}

 

 

#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);

        }

 

}

#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);

}