STM32爬坡寻迹小车(PID算法)

        由于该题我们没有去参加比赛,只是做着好玩,所以就没有按照竞赛题目的要求用msp430作为主控芯片,而是用了一块stm32最小系统板,题目相信大家都清楚了,下面是设计步骤:

一、简要分析

1、小车需要沿着黑虚线行驶,那么必须要有寻迹功能,并且行驶一定距离后还需要转向,那么最好使用三个红外模块可以更加准确的检测黑线。

2、小车需要爬不同角度的坡,那必然就不可能固定它的速度(这里的速度实质上是指他的占空比,因为在我的设计中速度其实是固定的,说速度是为了便于理解),因为如果固定的速度偏小,那么过高的坡它爬不上去,如果固定的速度过大,那么过低的坡它行驶的太快的话可能会不便于红外模块的接收。

3、正是由于第二点的要求所以需要引入了PID来调速,PID的作用就是为了使小车能够在爬不同角度的坡时能够大概的保持速度不变,不会因为坡度的提升而使速度降低,这里说速度大概保持不变是因为该题对小车行驶速度的变化要求不高,所以对PID参数的调节并不需要特别准确。

4、最后就是当三个红外模块全部检测到黑线时小车立即停止,蜂鸣器响。

二、设计思路

1、红外模块:使用PB12(左)、PB13(中)、PB14(右)三个GPIO口作为红外的输入检测;只有在需要转向的时候右边的红外模块才会检测到黑线,当它需要转向了,输出的低电平就会反馈到stm32中,再通过调节输出pwm的那几个GPIO口的占空比来控制四个电机的转速,从而实现转向的功能。并且当三个红外全部检测到黑线,即全部输出高电平时,使四个电机反转,注意这个反转的时间很小,占空比也很小,过了这段时间后马上将占空比调节为0。

2、由于stm32最小系统板只有三个通用定时器(TIM2、TIM3、TIM4)和一个高级定时器(TIM1),而我们总共需要用到四个定时器,那么我直接将TIM1的四个通道作为PWM输出控制四个电机,需要注意的是使用高级定时器输出PWM时需加上TIM_CtrlPWMOutputs这个函数;

3、TIM2和TIM3配置为编码器模式,TIM2的通道1和通道2作为小车左后轮编码器A、B相,A、B相接PA0、PA1;将TIM3的通道1和通道2作为小车右后轮编码器A、B相,A、B相接PA6、PA7;

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

5、随着角度的提升,小车的速度会越来越慢,以至于后面会爬不上去,但是如果初始占空比给大了,速度又太快,可能导致红外模块刚检测到黑线还没反应过来,小车就已经冲过去了,那还怎么玩是吧。但是有了PID控速的话,只要给了他一个初始速度,他就能自己通过调节占空比来大概的保持这个速度,当编码器测得的实际速度过小时,通过PID算法(增量式用于测速较好),将得到的一个占空比更大的PWM波返回,达到增加速度的目的。

6、使用TB6612作为电机的驱动模块,使用LM2596S降压模块稳出5V电压给各模块供电。

二、硬件电路:

1、电机驱动的选择
我们采用的是TB6612FNG电机驱动模块。TB6612FNG的主要引脚功能:
(1)AINl/AIN2、BIN1/BIN2、PWMA/PWMB为控制信号输入端;
(2)AO1/A02、B01/B02为2路电机控制输出端;
(3)STBY为正常工作/待机状态控制引脚;
(4)VM(3~13.5 V)和VCC(2.7~5.5 V)分别为电机驱动电压输入和逻辑电平输入端。

2、红外模块的选择

该红外循迹模块用于小车行驶过程中检测前方道路两端的黑线,并将相应的信息发送到STM32单片机。单片机实时接收,当左红外探测到黑线,则表明左轮即将压线,需要向左调整方向;当右红外探测到黑线,则表明右轮即将压线,需要向右调整方向。根据不断接收来自红外模块发送的信息,即可实时调整方向,从而保证小车不偏离轨道,能够按照规定的路线行驶。

3、编码器电机的选择

该霍尔编码器工作电压为5V,编码器线数为260线,车轮转一圈,电机输出260个脉冲,倍频后为1040,精度为0.35,可较为准确的获取该小车的实际速度,为小车的精准控速提供基本的硬件支持。

由于stm32最小系统板内部资源及硬件的限制,我们只使用两个后轮的编码器来采集速度, 并且值得注意的是,编码器有个转速上限,超过这个上限是不能正常工作的,这个是硬件的限制,原则上线数越多转速就越低,我们买的是260线的,经过软件四倍频就是1024,也就是小车转一圈cnt的值为1024。

接线方法:

  1. 电机线:接电机驱动的输出口1
  2. 编码器电源:接5V(3.3V也可)
  3. 编码器输出A相:接单片机
  4. 编码器输出B相:接单片机
  5. 编码器地线:接地
  6. 电机线+:接电机驱动的输出口2

4、降压模块的选择

采用LM2596S稳出5V电压。LM2596是非同步降压型电源管理单片集成电路的开关电压调节器,能够输出3A的驱动电流,同时具有很好的线性和负载调节特性。LM2596内部包含150KHZ振荡器、1.23v基准稳压电路、热关断电路、电流限制电路、放大器、比较器和内部稳压电路等。可调范围1.23~37V。

5、车轮及车胎的选择
我认为,这个题目中,车轮及车胎的选择至关重要,一定要选择摩擦力足够大的车胎,毕竟想要获得高分,你的车就必须要能爬上30度的坡。

三、TIM配置步骤

1 TIM1高级定时器):

功能:TIM1的四个通道(CH1-CH4)分别用于对小车四个电机的PWM输出,通过调节占空比,改变电机的转速,从而对小车进行调速。

配置步骤:

  1. 使能定时器及端口时钟,设置对应引脚复用映射;
  2. 初始化定时器参数,包含自动重装值,分频系数,计数方式等;
  3. 初始化通道输出比较参数,包含PWM模式,输出极性,使能等;
  4. 修改TIMX_CCRX的值,控制占空比(一般在main函数中实时修改);
  5. 使能TIMX在CCRX上的预装载寄存器;
  6. 使能TIMX在ARR上的预装载寄存器允许位;
  7. 开启定时器。

定时器基本结构体中,将预分频值设置为720,即将系统时钟72MKZ分频为 72M / 720 = 100KHZ ,为TB6612电机驱动PWM输出的最大频率;重装载值设置为20000,使可调节占空比的数据范围更大,更适合于PID算法的转换;计数方式为向上计数。

通道输出比较结构体中,将PWM模式设置为PWM1,输出极性设置为输出高电平(将模式设置为PWM2,极性设置为低电平效果一致)。

另由于TIM1是高级定时器,配置过程中需要将相应的结构体成员全部配置,配置参数与上类似。

2 TIM2、TIM3

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

配置步骤:

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

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

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

3 TIM4

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

配置步骤:

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

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

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

四、实物图

五、软件部分代码

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

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


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

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




本图文内容来源于网友网络收集整理提供,作为学习参考使用,版权属于原作者。
THE END
分享
二维码
< <上一篇
下一篇>>