代码收藏家 STM32智能循迹避障小车（1）：实现循迹调速功能详解
一.硬件的选择与连接
1.stm32f103rct6最小系统板
2.两个TB6612FNG电机驱动模块
线路连接请参考实际程序连线，此图为网上下载仅供参考。
3.三个TCRT5000循迹模块
这种是单路TCRT5000循迹模块，有四个接口，VCC电源接口、GND地接口，DO就是它的输出信号，然后AO口（模拟信号输出）。一般DO口接在stm32系统板上。我们可以看到下面有两个LED，一个是接上VCC、GND通电之后就亮了，另外一个没有遇到黑线就亮,DO口输出一个低电平，遇到黑线就熄灭，同时DO口输出一个高电平。上面还有一个可以调节的滑动变阻器，它可以调节红外检测距离（调节灵敏度）。
二.原理介绍
1.TB6612FNG电机驱动模块
为什么需要电机驱动芯片？
直流电机是一种将电能转换为机械能的装置，有两个电极，当电极正接时，电机正转，当电极反接时，电机反转，直流电机属于大功率器件，GPIO口无法直接驱动，需要配合电机驱动电路作。
TB6612是一款双路H桥型的直流电机驱动芯片，可以驱动两个直流电机并且控制其转速和方向。
TB6612驱动芯片的使用：
1.可以同时驱动两个电机AB。
2.PWMA/PWMB为两个电机提供pwm脉冲。
3.AIN1/AIN2,BIN1/BIN2.控制电机的正反转和停止。
4.STBY可以理解为一个使能端口，高电平有效。
使用方法为 ： STBY高电平，提供pwm脉冲，设置A/BIN控制正反转。
2.TCRT5000循迹模块
工作电压 3.3V-5V
检测范围：1mm-25mm
输出形式 ：数字开关量输出（0 和 1）
TCRT5000 传感器的红外发射二极管不断发射红外线，当发射出的红外线没有被反射回来或被反射回来但强度不够大时，红外接收管一直处于关断状态，此时模块的输出端为高电平，指示二极管一直处于熄灭状态；被检测物体出现在检测范围内时，红外线被反射回来且强度足够大，红外接收管饱和，此时模块的输出端为低电平，指示二极管被点亮。
接线：
1、VCC:接电源正极（3-5V）
2、GND:接电源负极
3、DO:TTL 开关信号输出 （GPIO口）
4、AO:模拟信号输出
三.循迹小车的实现
1.逻辑状态图
2.红外循迹小车原理
循迹模块我用的是TCRT5000传感器。黑线的检测原理是红外发射管发射光线到路面，红外光遇到白底则被反射，接收管接收到反射光，经施密特触发器整形后输出低电平，指示灯点亮；当红外光遇到黑线时则被吸收，接收管没有接收到反射光，经施密特触发器整形后输出高电平，输出指示灯熄灭。简单的说就是当红外寻迹模板遇见黑线时会产生一个高电平，遇见白线时会返回一个低电平。所以根据原理设计思路为当左侧红外传感器遇见黑线时左拐，右边红外传感器遇见黑线时右拐。这样就可以完成小车寻迹。
当然除了遇到黑线熄灭，当距离太远红外线反射后检测不到，此时指示灯也会熄灭。那么如果要循迹，模块离地面要近，在没有遇到黑线时确保指示灯长亮，一旦指示灯熄灭就说明遇到黑线了。
三、寻迹小车设计注意事项
（1）因为硬件条件有限，反应速度不是很快会有一定的误差，所以小车的速度要尽量慢下来，从而弥补硬件的不足让小车有足够的反应时间。
（2）在设置三个红外传感器的IO口模式时要设置为浮空输入，这样才能通过程序读取IO口的状态来判断。浮空输入模式下，I/O端口的电平信号直接进入输入数据寄存器。也就是说，I/O的电平状态是不确定的，完全由外部输入决定；如果在该引脚悬空（在无信号输入）的情况下，读取该端口的电平是不确定的。
（3）在测试小车时尽量在光线较暗的条件下来测试小车，避免光线过亮影响测试。
（4）循迹模块依次连接 PB3 、PB4 、PB5 ，但发现PB3，PB4引脚默认高电平，对IO控制不起作用，考虑是否引脚复用了，在参考芯片手册后，发现该引脚复用了。如下图：
由下图可以看到PB3/PB4默认功能是JTAG功能，不是GPIO功能，需要对其进行配置使其作为普通IO。
解决方法如下：
1.开启复用时钟： 要将PB3/PB4作为普通IO使用，需要进行复用操作，所以要开启复用时钟。
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
2.关闭JTAG功能： PB3/PB4用作JTAG的调试引脚，为将其用作GPIO，需要关闭JTAG功能。
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
3.正常配置IO： 实现上述操作后，我们可以正常配置IO口。
因此我们小组查找了其它复用引脚，当处于完全复位状态，PA13，PA14，PA15，PB3，PB4都是不可以直接作为普通IO或者其他功能来使用的。
四.关键代码实现
1.电机调速代码
motor.h
#ifndef __MOTOR_H_
#define __MOTOR_H_
#include "stm32f10x.h"
void motor_Init(void);
void TIM4_PWM_Init(unsigned short arr,unsigned short psc);//设置PWM1比较值 为Compare 即输出比较值
void PWM_SetCompare_LD(uint16_t Compare);
void PWM_SetCompare_LU(uint16_t Compare);
void PWM_SetCompare_RD(uint16_t Compare);//设置PWM比较值 为Compare
void PWM_SetCompare_RU(uint16_t Compare);//设置PWM比较值 为Compare
void Motor_RD_SetSpeed(int8_t Speed);//设置右后路电机速度 PWM
void Motor_LD_SetSpeed(int8_t Speed);//设置左后路电机PWM 速度
void Motor_RU_SetSpeed(int8_t Speed);//设置右前路电机速度 PWM
void Motor_LU_SetSpeed(int8_t Speed);//设置左前路电机PWM 速度
void Car_Stop(int8_t speed);//小车停止
void Car_Up(int8_t speed);//小车前进
void Car_Down(int8_t speed);//小车后退
void Car_TurnRight_1(int8_t speed);//小车右转
void Car_TurnLeft_1(int8_t speed);//小车左转
void Car_TurnRight_2(int8_t speed);//小车右转
void Car_TurnLeft_2(int8_t speed);//小车左转
void Car_TurnRight_3(int8_t speed);//小车右转
void Car_TurnLeft_3(int8_t speed);//小车左转
void Car_TurnRight_Angle(int8_t speed);//小车右转(直角弯)
void Car_TurnLeft_Angle(int8_t speed);//小车左转(直角弯)
void Car_Spin(int8_t speed);//小车旋转
/************************电机驱动1*******************************/
//右后
#define IN1_PIN GPIO_Pin_0
#define IN1_PORT GPIOA
#define IN1_CLK RCC_APB2Periph_GPIOA
#define IN2_PIN GPIO_Pin_1
#define IN2_PORT GPIOA
#define IN2_CLK RCC_APB2Periph_GPIOA
//左后
#define IN3_PIN GPIO_Pin_2
#define IN3_PORT GPIOA
#define IN3_CLK RCC_APB2Periph_GPIOA
#define IN4_PIN GPIO_Pin_3
#define IN4_PORT GPIOA
#define IN4_CLK RCC_APB2Periph_GPIOA
/*************************电机驱动2********************************/
//右前
#define IN1_1PIN GPIO_Pin_4
#define IN1_1PORT GPIOA
#define IN1_1CLK RCC_APB2Periph_GPIOA
#define IN2_2PIN GPIO_Pin_5
#define IN2_2PORT GPIOA
#define IN2_2CLK RCC_APB2Periph_GPIOA
//左前
#define IN3_3PIN GPIO_Pin_6
#define IN3_3PORT GPIOA
#define IN3_3CLK RCC_APB2Periph_GPIOA
#define IN4_4PIN GPIO_Pin_7
#define IN4_4PORT GPIOA
#define IN4_4CLK RCC_APB2Periph_GPIOA
//电机调速PWM驱动IO定义
//左前
#define LEFT_MOTOR_PWM_1 GPIO_Pin_6
#define LEFT_MOTOR_PWM_1_GPIO GPIOB
#define LEFT_MOTOR_PWM_1_SET GPIO_SetBits(LEFT_MOTOR_PWM_1_GPIO,LEFT_MOTOR_PWM_1)
#define LEFT_MOTOR_PWM_1_RESET GPIO_ResetBits(LEFT_MOTOR_PWM_1_GPIO,LEFT_MOTOR_PWM_1)
//右前
#define RIGHT_MOTOR_PWM_1 GPIO_Pin_7
#define RIGHT_MOTOR_PWM_1_GPIO GPIOB
#define RIGHT_MOTOR_PWM_1_SET GPIO_SetBits(RIGHT_MOTOR_PWM_1_GPIO,RIGHT_MOTOR_PWM_1)
#define RIGHT_MOTOR_PWM_1_RESET GPIO_ResetBits(RIGHT_MOTOR_PWM_1_GPIO,RIGHT_MOTOR_PWM_1)
//左后
#define LEFT_MOTOR_PWM_2 GPIO_Pin_8
#define LEFT_MOTOR_PWM_2_GPIO GPIOB
#define LEFT_MOTOR_PWM_2_SET GPIO_SetBits(LEFT_MOTOR_PWM_2_GPIO,LEFT_MOTOR_PWM_2)
#define LEFT_MOTOR_PWM_2_RESET GPIO_ResetBits(LEFT_MOTOR_PWM_2_GPIO,LEFT_MOTOR_PWM_2)
//右后
#define RIGHT_MOTOR_PWM_2 GPIO_Pin_9
#define RIGHT_MOTOR_PWM_2_GPIO GPIOB
#define RIGHT_MOTOR_PWM_2_SET GPIO_SetBits(RIGHT_MOTOR_PWM_2_GPIO,RIGHT_MOTOR_PWM_2)
#define RIGHT_MOTOR_PWM_2_RESET GPIO_ResetBits(RIGHT_MOTOR_PWM_2_GPIO,RIGHT_MOTOR_PWM_2)
#endif
motor.c
#include "motor.h"
#include "Math.h"
#include "delay.h"
#include "stm32f10x.h" // Device header
void motor_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
RCC_APB2PeriphClockCmd(IN1_CLK, ENABLE);
GPIO_InitStruct.GPIO_Pin = IN1_PIN | IN2_PIN | IN3_PIN | IN4_PIN ;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(IN1_PORT, &GPIO_InitStruct);
RCC_APB2PeriphClockCmd(IN1_1CLK, ENABLE);
GPIO_InitStruct.GPIO_Pin = IN1_1PIN | IN2_2PIN | IN3_3PIN | IN4_4PIN;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(IN1_1PORT, &GPIO_InitStruct);
}
void TIM4_PWM_Init(unsigned short arr,unsigned short psc)
{
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);//
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //使能GPIO外设时钟使能
GPIO_InitStructure.GPIO_Pin = LEFT_MOTOR_PWM_1; //左前电机PWM控制 PB1
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_Init(LEFT_MOTOR_PWM_1_GPIO, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = RIGHT_MOTOR_PWM_1; //右前电机PWM控制 PB0
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_Init(RIGHT_MOTOR_PWM_1_GPIO, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = LEFT_MOTOR_PWM_2; //左后电机PWM控制 PB8
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_Init(LEFT_MOTOR_PWM_2_GPIO, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = RIGHT_MOTOR_PWM_2; //右后电机PWM控制 PB9
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_Init(RIGHT_MOTOR_PWM_2_GPIO, &GPIO_InitStructure);
TIM_InternalClockConfig(TIM4);
TIM_OCStructInit(&TIM_OCInitStructure);//补全结构体中未配置参数
TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值80K
TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置用来作为TIMx时钟频率除数的预分频值 不分频
TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //选择定时器模式:TIM脉冲宽度调制模式1
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_Pulse = 0; //设置待装入捕获比较寄存器的脉冲值
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //输出极性:TIM输出比较极性高
TIM_OC2Init(TIM4, &TIM_OCInitStructure); //根据TIM_OCInitStruct中指定的参数初始化外设TIMx
TIM_OC1Init(TIM4, &TIM_OCInitStructure); //根据TIM_OCInitStruct中指定的参数初始化外设TIMx
TIM_OC3Init(TIM4, &TIM_OCInitStructure); //根据TIM_OCInitStruct中指定的参数初始化外设TIMx
TIM_OC4Init(TIM4, &TIM_OCInitStructure); //根据TIM_OCInitStruct中指定的参数初始化外设TIMx
TIM_CtrlPWMOutputs(TIM4,ENABLE); //MOE 主输出使能
TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable); //CH4预装载使能
TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable); //CH3预装载使能
TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable); //CH3预装载使能
TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable); //CH4预装载使能
TIM_ARRPreloadConfig(TIM4, ENABLE); //使能TIMx在ARR上的预装载寄存器
TIM_Cmd(TIM4, ENABLE); //使能TIM4
}
//设置PWM比较值 为Compare 即输出比较值
void PWM_SetCompare_LU(uint16_t Compare)
{
TIM_SetCompare1(TIM4, Compare); //PB6
}
//设置PWM比较值 为Compare
void PWM_SetCompare_RU(uint16_t Compare)
{
TIM_SetCompare2(TIM4, Compare); //PB7
}
//设置PWM比较值 为Compare 即输出比较值
void PWM_SetCompare_LD(uint16_t Compare)
{
TIM_SetCompare3(TIM4, Compare); //PB8
}
//设置PWM比较值 为Compare
void PWM_SetCompare_RD(uint16_t Compare)
{
TIM_SetCompare4(TIM4, Compare); //PB9
}
//设置右后电机速度 PWM
void Motor_RD_SetSpeed(int8_t Speed)
{
if (Speed >= 0)//Speed值为正
{
GPIO_ResetBits(IN1_PORT, IN1_PIN );//电机正转
GPIO_SetBits(IN2_PORT, IN2_PIN);
PWM_SetCompare_RD(Speed);//设置Speed转速
}
else//Speed值为负
{
GPIO_ResetBits(IN2_PORT, IN2_PIN);//电机反转
GPIO_SetBits(IN1_PORT, IN1_PIN);
PWM_SetCompare_RD(-Speed);//设为-Speed转速
}
}
//设置左后电机PWM 速度
void Motor_LD_SetSpeed(int8_t Speed)
{
if (Speed >= 0)
{
GPIO_ResetBits(IN3_PORT, IN3_PIN);
GPIO_SetBits(IN4_PORT, IN4_PIN);
PWM_SetCompare_LD(Speed);
}
else
{
GPIO_ResetBits(IN4_PORT, IN4_PIN);
GPIO_SetBits(IN3_PORT, IN3_PIN);
PWM_SetCompare_LD(-Speed);
}
}
//设置左前电机PWM 速度
void Motor_LU_SetSpeed(int8_t Speed)
{
if (Speed >= 0)
{
GPIO_ResetBits(IN4_4PORT,IN4_4PIN);
GPIO_SetBits(IN3_3PORT,IN3_3PIN);
PWM_SetCompare_LU(Speed);
}
else
{
GPIO_ResetBits(IN3_3PORT,IN3_3PIN);
GPIO_SetBits(IN4_4PORT,IN4_4PIN);
PWM_SetCompare_LU(-Speed);
}
}
//设置右前电机速度 PWM
void Motor_RU_SetSpeed(int8_t Speed)
{
if (Speed >= 0)//Speed值为正
{
GPIO_ResetBits(IN2_2PORT,IN2_2PIN);
GPIO_SetBits(IN1_1PORT,IN1_1PIN);//电机正转
PWM_SetCompare_RU(Speed);//设置Speed转速
}
else//Speed值为负
{
GPIO_ResetBits(IN1_1PORT,IN1_1PIN);//电机反转
GPIO_SetBits(IN2_2PORT,IN2_2PIN);
PWM_SetCompare_RU(-Speed);//设为-Speed转速
}
}
void Car_Stop(int8_t speed)//小车停止
{
Motor_RU_SetSpeed(0);
Motor_RD_SetSpeed(0);
Motor_LU_SetSpeed(0);
Motor_LD_SetSpeed(0);
delay_ms(10);
}
void Car_Up(int8_t speed)//小车前进
{
Motor_RD_SetSpeed(speed);//设置右后路电机速度
Motor_LD_SetSpeed(speed);//设置左后路电机速度
Motor_RU_SetSpeed(speed);//设置右前路电机速度
Motor_LU_SetSpeed(speed);//设置左前路电机速度
// delay_ms(10);
}
void Car_Down(int8_t speed)//小车后退
{
Motor_RD_SetSpeed(-speed);//设置右后路电机速度
Motor_LD_SetSpeed(-speed);//设置左后路电机速度
Motor_RU_SetSpeed(-speed);//设置右前路电机速度
Motor_LU_SetSpeed(-speed);//设置左前路电机速度
// delay_ms(10);
}
void Car_TurnRight_1(int8_t speed)//小车右转
{
Motor_RU_SetSpeed(speed-18);
Motor_RD_SetSpeed(speed-18);
Motor_LU_SetSpeed(speed);
Motor_LD_SetSpeed(speed);
// delay_ms(10);
}
void Car_TurnLeft_1(int8_t speed)//小车左转
{
Motor_RU_SetSpeed(speed);
Motor_RD_SetSpeed(speed);
Motor_LU_SetSpeed(speed-18);
Motor_LD_SetSpeed(speed-18);
// delay_ms(10);
}
void Car_TurnRight_2(int8_t speed)//小车右转
{
Motor_RU_SetSpeed(0);
Motor_RD_SetSpeed(0);
Motor_LU_SetSpeed(speed);
Motor_LD_SetSpeed(speed);
// delay_ms(10);
}
void Car_TurnLeft_2(int8_t speed)//小车左转
{
Motor_RU_SetSpeed(speed);
Motor_RD_SetSpeed(speed);
Motor_LU_SetSpeed(0);
Motor_LD_SetSpeed(0);
// delay_ms(10);
}
void Car_TurnRight_3(int8_t speed)//小车右转
{
Motor_RU_SetSpeed(speed-35);
Motor_RD_SetSpeed(speed-35);
Motor_LU_SetSpeed(speed);
Motor_LD_SetSpeed(speed);
// delay_ms(10);
}
void Car_TurnLeft_3(int8_t speed)//小车左转
{
Motor_RU_SetSpeed(speed);
Motor_RD_SetSpeed(speed);
Motor_LU_SetSpeed(speed-24);
Motor_LD_SetSpeed(speed-24);
// delay_ms(10);
}
void Car_TurnRight_Angle(int8_t speed)//小车右转(直角弯)
{
Motor_RU_SetSpeed(-speed);
Motor_RD_SetSpeed(-speed);
Motor_LU_SetSpeed(speed);
Motor_LD_SetSpeed(speed);
// delay_ms(10);
}
void Car_TurnLeft_Angle(int8_t speed)//小车左转(直角弯)
{
Motor_RU_SetSpeed(speed);
Motor_RD_SetSpeed(speed);
Motor_LU_SetSpeed(-speed);
Motor_LD_SetSpeed(-speed);
// delay_ms(10);
}
2.循迹模块代码
follow.h
#ifndef __FOLLOW_H
#define __FOLLOW_H
#include "stm32f10x.h"
#define LEFT_PIN GPIO_Pin_3
#define LEFT_PORT GPIOB
#define LEFT_CLK RCC_APB2Periph_GPIOB
#define MID_PIN_1 GPIO_Pin_4
#define MID_PORT_1 GPIOB
#define MID_CLK_1 RCC_APB2Periph_GPIOB
#define RIGHT_PIN GPIO_Pin_5
#define RIGHT_PORT GPIOB
#define RIGHT_CLK RCC_APB2Periph_GPIOB
#define L1 GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_3)
#define M1 GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_4)
#define R1 GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_5)
#define TC 100*L1+10*M1+1*R1
void follow_Init(void);
void follow_1(void);
void follow_2(void);
void follow_3(void);
#endif
follow.c
#include "stm32f10x.h" // Device header
#include "follow.h"
#include "motor.h"
#include "Delay.h"
int left=0,right=0;
int Speed=0;
void follow_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
//将管脚PB4特殊功能关掉
// GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST,ENABLE);
//将管脚PB3，PB4,PA15,特殊功能关掉
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);
RCC_APB2PeriphClockCmd(LEFT_CLK, ENABLE);
GPIO_InitStruct.GPIO_Pin = LEFT_PIN |MID_PIN_1 |RIGHT_PIN;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(LEFT_PORT, &GPIO_InitStruct);
}
//黑，高（1），灭
//白，低（0），亮
void follow_1(void)
{
if( TC == 111 || TC == 10 )//111 010
{
Car_Up(50);//小车前进
}
else if(TC == 100 || TC == 110)
{
Car_TurnLeft_1(50);//小车左转
}
else if(TC == 1 || TC == 11)//001
{
Car_TurnRight_1(50);//小车右转
}
}
//黑，高（1），灭
//白，低（0），亮
void follow_2(void)
{
if( TC == 111 || TC == 10 )//111 010
{
Car_Up(34);//小车前进
}
else if(TC == 1)//001
{
Car_TurnRight_2(34);//小车右转
}
else if(TC == 11)//011
{
Car_TurnRight_2(34);//小车右转
right = 1;
}
else if(TC == 100)
{
Car_TurnLeft_2(34);//小车左转
}
else if(TC == 110)
{
Car_TurnLeft_2(34);//小车左转
left = 1;
}
else if(TC == 000)
{
if(right==1)
{
while(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_4) == 0)
{
Car_TurnRight_Angle(25);//小车右转(直角弯)
}
right = 0;
}
else if(left==1)
{
while(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_4) == 0)
{
Car_TurnLeft_Angle(25);//小车左转(直角弯)
}
left =0;
}
}
}
//黑，高（1），灭
//白，低（0），亮
void follow_3(void)
{
if( TC == 111 || TC == 10 )//111 010
{
Car_Up(35);//小车前进
}
else if(TC == 100)
{
Car_TurnLeft_3(35);//小车左转
}
else if(TC == 110)
{
Car_TurnLeft_3(35);//小车左转
left = 1;
}
else if(TC == 1)//001
{
Car_TurnRight_3(35);//小车右转
}
else if(TC == 11)//011
{
Car_TurnRight_3(35);//小车右转
right = 1;
}
else if(TC == 000)
{
if(right==1)
{
while(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_4) == 0)
{
Car_TurnRight_Angle(25);//小车右转(直角弯)
}
right = 0;
}
else if(left==1)
{
while(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_4) == 0)
{
Car_TurnLeft_Angle(25);//小车左转(直角弯)
}
left =0;
}
}
}
