基于PID算法下STM32控制的坡道行驶电动小车(含源码)

---

前言

本题源于2020年TI杯大学生电子设计竞赛C题-----坡道行驶电动小车

由于手上没有MSP430/msp432 板子，所以本篇采用stm32实现

一、题目设计

任务
利用 TI 的 MSP430/msp432 平台，设计制作一个四轮电动小车。要求小车能沿着指定路线在坡道上自动循迹骑线行驶。小车必须独立运行，车外不能使用任何设备（包括电源）。小车（含电池）重量小于 1.5kg，外形尺寸在地面投影不大于 25cm×25cm。坡道用长、宽约 1m 的细木工板制作，允许板上有木质本色及自然木纹。木工板表面铺设画有 1cm×1cm 黑白间隔的纸条（以下简称为标记线）作为路线指示；标记线起始段为直线，平行于木板两边；标记线在坡顶转向 90°，转弯半径 20cm；标记线平行坡顶距≥30cm，距坡顶距离≤20cm；标记线总长度为 1m。停车标记为宽 1cm 长 5cm 的黑色线条，垂直于坡顶标记线。小车坡度

角示意及行驶线路顶视图如图 1 所示。

要求

（1） 坡度角 θ=0°，电动小车能够沿标记线自动骑线行驶，在停车点停车； 小车上标记点到停车标记中心线的垂直距离误差≤2cm。停车时立即发出声音提示。小车行驶过程中，其地面投影不得脱离标记线。（15 分）

（2） 在完成（1）的基础上，电动小车能够设定行驶时间，自动控制小车匀速通过 1 米长的线路，在停车点停车。行驶时间可在 10s~20s 间设定。误差绝对值≤1s。行驶过程中不得碾压、脱离标记线。时间误差每超过1s 扣 1 分。 （20 分）

（3） 坡度角 θ=10°，完成要求（2）的动作。 （20 分）

（4） 可任意指定坡度角 θ 在 11°~30°，完成要求（2）的动作。 （20 分）

（5） 在完成（4）后，尽量增加坡度角 θ，完成要求（2）动作。 （20 分）

（6） 其他。 （5 分）

（7） 设计报告： （20 分）

二、硬件选择

1. 主控芯片
采用STM32F103c8t6作为主控芯片

2. 电机
采用霍尔编码器电机（4个）

3. 电机驱动
需要采用2个TB6612电机驱动，分别控制左前轮左后轮、右前轮右后轮

4. 轮胎
选用黑色轮胎 海绵内胎，软橡胶外胎，金属轮毂（４个）

5. 红外传感器
采用三路循迹模块

6. 小车底板
采用铝合金底板

7. 电池
采用7.4v 3600mah锂电池

8. 面包板
插接导线，搭建电路

9. 下载器
采用ST-LINK V2仿真器

10. 其余工具零件
若干杜邦线、螺母、铜柱、螺丝刀、剥线钳等

实物图：

三、软件设计

1. stm32最小系统板
插在面包板上，方便引脚连接。由于stm32最小系统板没有基本定时器，所以最多可采用4个定时器( 1个高级定时器TIM1和3个通用定时器TIM2、TIM3、TIM4 )，在这里需要全部用上。

2. 定时器
TIM1：配置四个电机的PWM输出
左前：PA8 左后：PA9 右前：PA10 右后：PA11

TIM2：配置编码器模式，通道1和通道2分别作为小车左后轮编码器A、B相
PA0、PA1

TIM3：配置编码器模式，通道1和通道2分别作为小车右后轮编码器A、B相
PA6、PA7

TIM4：在中断读取TIM2、TIM3计数器（cnt）的值，设置TIM4每隔5ms溢出一次，每向上计数四次cnt的值加1，并且在中断函数中读取完cnt的值后就清零，那么每次进入TIM4溢出中断时，cnt的值都不会太大，可直接将cnt的值作为小车的当前速度。

3. 三路红外传感器
Vcc和GND连接TB6612模块上的Vcc和GND
红外输出： 左 中 右 分别接线PA2、PA3、PA4
(1) 当左右均未踩到黑线时，小车匀速直行
(2) 当左没踩到，右踩到黑线时，小车右转
(3) 当左踩到，右没踩到黑线时，小车左转
(4) 当左中右都踩到黑线时，小车停止

4. TB6612驱动
逻辑输入：
左前轮 配置为通道1　连接PB12,PB13
左后轮 配置为通道2　连接PB10,PB11
右前轮 配置为通道3　连接PB14,PB15
右后轮 配置为通道4　连接PB0,PB1

用锂电池给TB6612模块供电，即锂电池的正负极连接TB6612的Vin+和-；然后用该TB6612模块的Vout+和-连接另一块TB6612的Vin+和-，相继再用Vcc和GND与面包饭上的Vcc和GND相连，实现供电。

四、程序设计

1.　红外传感设计

void GPIOB_Init(void)
{
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
	
	GPIO_InitTypeDef GPIO_InitStructure;
	/*红外out引脚：PA2,PA3,PA4*/
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;//浮空输入
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_15;
	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_10 | GPIO_Pin_11 | GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB,&GPIO_InitStructure);
	/*初始为低电平*/
	GPIO_ResetBits(GPIOB,GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_10 | GPIO_Pin_11 | GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15);

}

void Change_Target(int t_l,int t_r)
{
	target_left = t_l;
	target_right = t_r;
}

void forward(int t)
{
	Change_Target(t,t);
	GPIO_SetBits(GPIOB,GPIO_Pin_1| GPIO_Pin_11 | GPIO_Pin_13 | GPIO_Pin_15);
	GPIO_ResetBits(GPIOB,GPIO_Pin_0| GPIO_Pin_10 | GPIO_Pin_12 | GPIO_Pin_14);
}

void back(int t)
{
	Change_Target(t,t);
	GPIO_SetBits(GPIOB,GPIO_Pin_0| GPIO_Pin_10 | GPIO_Pin_12 | GPIO_Pin_14);
	GPIO_ResetBits(GPIOB,GPIO_Pin_1| GPIO_Pin_11 | GPIO_Pin_13 | GPIO_Pin_15);
}

void turn_left(int t_l,int t_r)
{
	Change_Target(t_l,t_r);
	GPIO_SetBits(GPIOB,GPIO_Pin_1| GPIO_Pin_11 | GPIO_Pin_12 | GPIO_Pin_15);
	GPIO_ResetBits(GPIOB,GPIO_Pin_0| GPIO_Pin_10 | GPIO_Pin_13 | GPIO_Pin_14);
}

void turn_right(int t_l,int t_r)
{
	Change_Target(t_l,t_r);
	GPIO_SetBits(GPIOB,GPIO_Pin_1| GPIO_Pin_11 | GPIO_Pin_13 | GPIO_Pin_14);
	GPIO_ResetBits(GPIOB,GPIO_Pin_0| GPIO_Pin_10 | GPIO_Pin_12 | GPIO_Pin_15);
}

void stop(void)
{
	GPIO_ResetBits(GPIOB,GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_10 | GPIO_Pin_11 | GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15);
}

2.　定时器

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_Set;  //选择互补空闲状态下的非工作状态 低电平
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;//选择PWM1模式
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;//输出极性：高电平有效
	TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_Low;
	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);
}

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_Rising, TIM_ICPolarity_Rising);//TI12:A、B项都计数
    TIM_ICStructinit(&TIM_ICInitStructure);//缺省输入
    TIM_ICInitStructure.TIM_ICFilter = 10;  //滤波器
    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);
}

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

3.　PID设计

int target_left,target_right;
int err_Now1,err_Now2;
int err_last1,err_last2;
int err_last_last1,err_last_last2;
int spd_Now1,spd_Now2;
int jisuan1,jisuan2;
int out_left,out_right;
int cnt_left,cnt_right;

float Kp=0.5;
float Ki=0.4;
float Kd= 0.1;
		
		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);

4. main

int main(void)
{
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置中断优先级分组
	GPIOB_Init();
	
	TIM1_PWM_Init(100-1,72-1);
	TIM2_Init(65535,9);
	TIM3_Init(65535,9);
	TIM4_Init(5000,72-1); 
	int i;
	
	while(1)
	{
		if(L==0 && R==0)  i=0;
		
		if(L==1 && R==0)  i=1;
		
		if(L==0 && R==1)  i=3;
		
		if(L==1 && M==1 && R==1)  i=5;
		
		switch(i)
		{
			case 0:
				forward(4);
			  break;
			
			case 1:
				turn_left(4,10);	
			  break;

			case 3:
				turn_right(10,4);
			  break;

			case 5:
				stop();
				delay_ms(1000);
			  break;
		}		
	}
}

链接：https://pan.baidu.com/s/1DG2heIwx8T2kKFG0hiFcgA?pwd=n8il
提取码：n8il