遥控小车开发
前言
本文主要为参加课程《机器人设计与制作》的课程作品设计,选题以及设计要求如下:
本作品开发过程,地盘设计参考博主化作尘的作品,遥控部分参考了皇家园林巡游者
的树莓派小车。
本文主要作用为:
- 作品的开发记录(编调,测试等记录)。
- 便于后来者在此基础上进一步开发。
一 树莓派的连接
因为树莓派很久没用了,忘记了以前的设置,大概需要找到它的密码以及wifi的设置。
我的树莓派密码为123,通过以太网直连的方式,登录路由器后台 查看IP地址进行SSH连接,然后可以设置相应的wifi配置,为了开发方便,设置了一个便携式热点,获取得到的IP在连接以太网的情况下可以通过ifconfig查找wlan0的ip地址,如下(进行记录,避免下一次忘记):
inet 192.168.182.143 netmask 255.255.255.0 broadcast 192.168.182.255
inet6 2408:8469:f00:de8f:e836:3d5:2a6a:e8f3 prefixlen 64 scopeid 0x0<global>
inet6 fe80::6818:ac4:f0aa:e3f0 prefixlen 64 scopeid 0x20<link>
ether e4:5f:01:8c:24:9b txqueuelen 1000 (Ethernet)
后面的开发全部使用便携式热点进行SSH连接开发。
二 树莓派与手机通信
这两个的通信首先把通信协议设置好,然后通信的框架直接根据
先记录好手机的IP地址,因为查找手机在局域网中的IP地址比较麻烦,所以,使用手机的蜂窝网的IP地址,实际上设置好IP地址以及端口号,该地址可以在手机热点的局域网中正常通信。
通信协议
首先两者的通信存在两个线程,并且都是单工通信,其一,树莓派通过UDP向手机发送图像信息,其二,手机通过UDP向树莓派发送指令。
图像传输
该过程比较简单,直接使用CV库对图像进行裁剪,然后利用UDP接口传输图像。
指令传输
该过程采用TCP的形式,树莓派作为TCP的服务端,开启TCP服务器,绑定IP和端口为:
TCP_HOST_IP="192.168.182.143"
TCP_HOST_PORT=5050
客服端输入该IP和端口即可完成连接。
只能短按,长按不能连续走。
三 树莓派与STMC8T6通信
通信协议
两者通过串口通信,设置独立数据报进行通信。
因为C8T6发送数据为字节数据,而树莓派为高级抽象数据,需要注意大小端以及字节转换的问题。
因为树莓派与单片机之间的通信为单工通信,可以使用单字节简单的控制协议。
//因为控制比较简单,通信协议完全可以设计为单命令控制
//对于电机: 前、后、左、右,四个指令 0x01,0x02,0x03,0x04,停止0x05
//对于舵机: 上、下,两个指令 0x06,0x07,保留
0x08指令
//对于控制指令:遥控开关,仅有一个指令 0x09.
//共有9个指令
四 STMC8T6底盘开发
接线规划
需求分析:
- 与上位机串口通信,USART1->PA9,PA10
- 电机驱动需要两路PWM波,TIM4_CH3,TIM4_CH4->PB8,PB9,此外PB12,PB13控制电机旋转方向
- 舵机驱动,一路PWM波,TIM2_CH1->PA0
- 二(四)路红传感器信号,PA4-PA7
- 内置LED,PC13,用于测试
采用四驱差速驱动出现摩擦力过大无法顺利转弯的问题
- 更换电机考虑使用万向轮构造三轮车,并且考虑使用编码器实现PID控制
两路编码轮的驱动使用使用6路定时器控制。其中电机驱动一路PWM,编码器两个定时器通道。
同时,为了顺利控制电机的正反转,需要四个GPIO控制电机的正反转信号。
在原来的基础上,需要增加,四路定时器的使用,目前可用PA2(TIM2_CH3),PA3(TIM2_CH4),PB6(TIM4_CH1),PB7(TIM4_CH2)
GPIO目前可增加使用PA11,PA12 - 增加传感器,将PA4-PA7全部使用。
先完成二轮的小车启动
- 先增加PA11,PA12 GPIO的初始化,实现小车的前后左右行进
- 增加PA5,PA6 传感器的初始化,增加可行性
- 增加编码器定时器接口
4.1 电机驱动
小车的运动模型为四轮差速,转弯通过左右轮差速来实现。首先,需要两路定时器产生PWM波。
PWM初始化程序:
#include "MotorDev.h"
//初始化定时器输出PWM
void MotorInit(u16 arr,u16 psc){
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); //①使能定时器 4 时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB|RCC_APB2Periph_AFIO, ENABLE); //①使能 GPIO B 和 AFIO 复用功能时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8; //分别对应TIM4_CH3
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure); //①初始化 GPIO B
GPIO_SetBits(GPIOB,GPIO_Pin_8);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9; //分别对应TIM4_CH4
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure); //①初始化 GPIO B
GPIO_SetBits(GPIOB,GPIO_Pin_9);
//初始化 TIM4
TIM_TimeBaseStructure.TIM_Period = arr; //设置在自动重装载周期值
TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置预分频值
TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM 向上计数模式
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); //③初始化 TIMx
//初始化 TIM4 Channe3|4 PWM 模式
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; //选择 PWM 模式 2
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //输出极性高
TIM_OC3Init(TIM4, &TIM_OCInitStructure); //④初始化外设 TIM4 OC3
TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable); //使能预装载寄存器
TIM_OC4Init(TIM4, &TIM_OCInitStructure); //④初始化外设 TIM4 OC4
TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable); //使能预装载寄存器
TIM_Cmd(TIM4, ENABLE); //⑤使能 TIM4
TIM_SetCompare3(TIM4,arr/4);//大概对应2.4V
TIM_SetCompare4(TIM4,arr/2);//大概对应1.6V
}
记录使用L298N驱动的测试情况
因为电机的引脚是反过来的,根据现有的线路连接。
IN1,IN2,IN3,IN4极性设置为:0,1,1,0
现更改为0,1,0,1则可以实现正向旋转
增加PB12,PB13控制方向,GPIO初始化为输出模式。
方向函数还需要更改速度值。
第二版驱动
第二版驱动把转弯的方式更改为左右轮正反向同时进行的方式,确保小车可以快速、及时转向。
#include "MotorDev.h"
extern u8 MODE;
//初始化定时器输出PWM
void MotorInit(u16 arr,u16 psc){
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); //①使能定时器 4 时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB|RCC_APB2Periph_AFIO, ENABLE); //①使能 GPIO B 和 AFIO 复用功能时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8; //分别对应TIM4_CH3
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure); //①初始化 GPIO B
GPIO_SetBits(GPIOB,GPIO_Pin_8);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9; //分别对应TIM4_CH4
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure); //①初始化 GPIO B
GPIO_SetBits(GPIOB,GPIO_Pin_9);
//初始化 TIM4
TIM_TimeBaseStructure.TIM_Period = arr; //设置在自动重装载周期值
TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置预分频值
TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM 向上计数模式
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); //③初始化 TIMx
//初始化 TIM4 Channe3|4 PWM 模式
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; //选择 PWM 模式 2
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //输出极性高
TIM_OC3Init(TIM4, &TIM_OCInitStructure); //④初始化外设 TIM4 OC3
TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable); //使能预装载寄存器
TIM_OC4Init(TIM4, &TIM_OCInitStructure); //④初始化外设 TIM4 OC4
TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable); //使能预装载寄存器
TIM_Cmd(TIM4, ENABLE); //⑤使能 TIM4
TIM_SetCompare3(TIM4,arr/4);//大概对应2.4V 对比值越小,输出电压越大,对应左轮
TIM_SetCompare4(TIM4,arr/4);//大概对应2.4V,PB9 对应右轮
}
void DirInit(void){
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //使能PB端口时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //使能PA端口时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12; //PB.12端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
GPIO_Init(GPIOB, &GPIO_InitStructure); //根据设定参数初始化GPIOB.12
GPIO_SetBits(GPIOB,GPIO_Pin_12); //PB.12 输出高
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13; //PB.13端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
GPIO_Init(GPIOB, &GPIO_InitStructure); //根据设定参数初始化GPIOB.12
GPIO_ResetBits(GPIOB,GPIO_Pin_13); //PB.13 输出低
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11; //PA.11端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOA.11
GPIO_SetBits(GPIOA,GPIO_Pin_11); //PA.11 输出高
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12; //PA.12端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOA.12
GPIO_ResetBits(GPIOA,GPIO_Pin_12); //PA.12 输出低
}
//PB.12 PB.13控制左轮电机旋转方向
//PB.12 ->1 PB.13->0 正向旋转
//PB.12 ->0 PB.13->1 反向旋转
//PB.12 ->0 PB.13->0 停止
//PB.12 ->1 PB.13->1 停止
//PA11,PA12控制右轮电机旋转方向
//PA.11 ->1 PA.12->0 正向旋转
//PA.11 ->0 PA.12->1 反向旋转
//PA.11 ->0 PA.12->0 停止
//PA.11 ->1 PA.12->1 停止
void GoForward(u16 Leftspeed,u16 Rightspeed){
GPIO_ResetBits(GPIOB,GPIO_Pin_13); //PB.13 输出低
GPIO_SetBits(GPIOB,GPIO_Pin_12); //PB.12 输出高
GPIO_ResetBits(GPIOA,GPIO_Pin_12); //PA.12 输出低
GPIO_SetBits(GPIOA,GPIO_Pin_11); //PA.11 输出高
TIM_SetCompare3(TIM4,Leftspeed);//大概对应2.4V 对比值越小,输出电压越大
TIM_SetCompare4(TIM4,Rightspeed);//大概对应2.4V
}
void GoBack(u16 Leftspeed,u16 Rightspeed){
GPIO_ResetBits(GPIOB,GPIO_Pin_12); //PB.12 输出低
GPIO_SetBits(GPIOB,GPIO_Pin_13); //PB.13 输出高
GPIO_ResetBits(GPIOA,GPIO_Pin_11); //PA.11 输出低
GPIO_SetBits(GPIOA,GPIO_Pin_12); //PA.12 输出高
TIM_SetCompare3(TIM4,Leftspeed);//大概对应2.4V 对比值越小,输出电压越大
TIM_SetCompare4(TIM4,Rightspeed);//大概对应2.4V
}
void GoStop(void){
GPIO_SetBits(GPIOB,GPIO_Pin_12); //PB.12 输出高
GPIO_SetBits(GPIOB,GPIO_Pin_13); //PB.13 输出高
GPIO_SetBits(GPIOA,GPIO_Pin_12); //PA.12 输出高
GPIO_SetBits(GPIOA,GPIO_Pin_11); //PA.11 输出高
}
//差速转弯,左转,右轮速度大 右轮正向,左轮反向
void GoLeft(u16 Leftspeed,u16 Rightspeed){
GPIO_SetBits(GPIOB,GPIO_Pin_13); //PB.13 输出高
GPIO_ResetBits(GPIOB,GPIO_Pin_12); //PB.12 输出低 左轮反向旋转
GPIO_ResetBits(GPIOA,GPIO_Pin_12); //PA.12 输出低
GPIO_SetBits(GPIOA,GPIO_Pin_11); //PA.11 输出高 右轮正向旋转
TIM_SetCompare3(TIM4,Leftspeed);//大概对应2.4V 对比值越小,输出电压越大
TIM_SetCompare4(TIM4,Rightspeed);//大概对应2.4V
}
//右转,左轮速度大 左轮正向旋转,右轮反向旋转
void GoRight(u16 Leftspeed,u16 Rightspeed){
GPIO_ResetBits(GPIOB,GPIO_Pin_13); //PB.13 输出低
GPIO_SetBits(GPIOB,GPIO_Pin_12); //PB.12 输出高
GPIO_SetBits(GPIOA,GPIO_Pin_12); //PA.12 输出高
GPIO_ResetBits(GPIOA,GPIO_Pin_11); //PA.11 输出低
TIM_SetCompare3(TIM4,Leftspeed);//大概对应2.4V 对比值越小,输出电压越大
TIM_SetCompare4(TIM4,Rightspeed);//大概对应2.4V
}
extern u16 SPEED;
extern u16 AutoSPEED;
extern u16 SuperSPEED;
extern u16 LeftPin1,RightPin1,LeftPin2,RightPin2;
//封装一个循迹函数
void FollowLine(void){
if(MODE==0){
GoStop();//先停一下
LeftPin1=GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_4);
RightPin1=GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_5);
LeftPin2=GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_6);
RightPin2=GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_7);
//意为产生中断闪烁
//要先判断是不是要停止
if(((LeftPin1||LeftPin2)&&(RightPin1||RightPin2))){
//有两个都检测到,遇到十字路口
LeftPin1=GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_4);
RightPin1=GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_5);
LeftPin2=GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_6);
RightPin2=GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_7);
//BlinkLED(10);
GoStop();
}
if(LeftPin2&&(!LeftPin1)&&(!RightPin1)&&(!RightPin2)){//只有左2被触发
//稍微偏左
//BlinkLED(1);
LeftPin1=0;
RightPin1=0;
LeftPin2=1;
RightPin2=0;
GoLeft(AutoSPEED,AutoSPEED);//半速转弯
}
if(RightPin2&&(!RightPin1)&&(!LeftPin1)&&(!LeftPin2)){//只有右2被触发
//稍微偏右
//BlinkLED(2);
LeftPin1=0;
RightPin1=0;
LeftPin2=0;
RightPin2=1;
GoRight(AutoSPEED,AutoSPEED);//半速转弯
}
if(LeftPin1&&(!LeftPin2)&&(!RightPin1)&&(!RightPin2)){//只有左1被触发
//非常偏左
//BlinkLED(5);
LeftPin1=1;
RightPin1=0;
LeftPin2=0;
RightPin2=0;
GoLeft(SuperSPEED,SuperSPEED);//全速转弯
//delay_ms(900);
}
if(LeftPin1&&LeftPin2&&(!RightPin1)&&(!RightPin2)){
LeftPin1=1;
RightPin1=0;
LeftPin2=1;
RightPin2=0;
GoLeft(SuperSPEED,SuperSPEED);//全速转弯,而且需要延时一下,否则直接冲出去了
//delay_ms(900);
}
if(RightPin1&&RightPin2&&(!LeftPin1)&&(!LeftPin2)){
LeftPin1=0;
RightPin1=1;
LeftPin2=0;
RightPin2=1;
GoRight(SuperSPEED,SuperSPEED);//全速转弯
//delay_ms(900);
}
if(RightPin1&&(!RightPin2)&&(!LeftPin1)&&(!LeftPin2)){//只有右1被触发
//非常偏右
//BlinkLED(8);
LeftPin1=0;
RightPin1=1;
LeftPin2=0;
RightPin2=0;
GoRight(SuperSPEED,SuperSPEED);//全速转弯
//delay_ms(900);
}
}
}
4.2 舵机驱动程序
舵机只需要一路PWM输出即可,初始化TIM2的通道1。
程序如下:
//初始化定时器输出PWM
void SteerInit(u16 arr,u16 psc){
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //使能定时器2时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO, ENABLE); //使能GPIO外设和AFIO复用功能模块时钟
//GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3, ENABLE); //Timer3部分重映射 TIM3_CH2->PB5 这里不需要映射
//设置该引脚为复用输出功能,输出TIM2 CH1的PWM脉冲波形 GPIOA.0
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0; //TIM_CH2
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIO
GPIO_SetBits(GPIOA,GPIO_Pin_0);
//初始化TIM2 周期3000/100000=30ms;
TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值
TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置用来作为TIMx时钟频率除数的预分频值
TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
//初始化TIM2 Channel-1 PWM模式
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; //选择定时器模式:TIM脉冲宽度调制模式2
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //输出极性:TIM输出比较极性高
TIM_OC1Init(TIM2, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM2 OC1
TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable); //使能TIM2在CCR1上的预装载寄存器
TIM_Cmd(TIM2, ENABLE); //使能TIM2
TIM_SetCompare1(TIM2,arr/4);//大概对应2.4V
}
4.3 UART1串口1驱动
串口初始化极其中断服务函数如下:
#include "sys.h"
#include "usart.h"
#include "MotorDev.h"
#include "SteerDev.h"
#include "delay.h"
#if EN_USART1_RX //如果使能了接收
//串口1中断服务程序
//注意,读取USARTx->SR能避免莫名其妙的错误
u8 USART_RX_BUF[USART_REC_LEN]; //接收缓冲,最大USART_REC_LEN个字节.
//接收状态
//bit15, 接收完成标志
//bit14, 接收到0x0d 即接收到结束字符
//bit13~0, 接收到的有效字节数目
u16 USART_RX_STA=0; //接收状态标记
extern u8 Dir;
extern u16 SPEED;
extern u8 MODE;
void My_USART1_Init(u32 bound){
//时钟使能
GPIO_InitTypeDef GPIO_InitStructure;//GPIO参初始化的第二个结构体参数
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//使能GPIOA的时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);//使能串口1的时钟
//GPIO初始化
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF_PP;//设置模式
GPIO_InitStructure.GPIO_Pin=GPIO_Pin_9;//设置引脚
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStructure);//第一个参数为GPIO信息,第二个参数的结构体需要自定义
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IPU;//设置模式
GPIO_InitStructure.GPIO_Pin=GPIO_Pin_10;//设置引脚
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStructure);//第一个参数为GPIO信息,第二个参数的结构体需要自定义
//串口初始化
USART_InitStructure.USART_BaudRate=bound;//设置波特率
USART_InitStructure.USART_HardwareFlowControl=USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode=USART_Mode_Rx|USART_Mode_Tx;//发送接收同时使能
USART_InitStructure.USART_Parity=USART_Parity_No;//设置奇偶校验
USART_InitStructure.USART_StopBits=USART_StopBits_1;//设置停止位
USART_InitStructure.USART_WordLength=USART_WordLength_8b;//设置字长
USART_Init(USART1,&USART_InitStructure);
//使能串口
USART_Cmd(USART1,ENABLE);
//可以不设置中断,但是下面要测试中断,现在主函数设置了中断的优先级
//开启需要的中断
USART_ITConfig(USART1,USART_IT_RXNE,ENABLE);//这里的第二个参数设置接收缓存区非空即中断
NVIC_InitStructure.NVIC_IRQChannel=USART1_IRQn;//设置通道,在stm32f10x.h文件里面找到相应的定义
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=1;//设置抢占优先级
NVIC_InitStructure.NVIC_IRQChannelSubPriority=1;//设置响应优先级
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;//是否开启中断通告
NVIC_Init(&NVIC_InitStructure);//中断设置
}
//下面编写中断服务函数
void USART1_IRQHandler(){
//首先判断中断类型
u8 Res;
if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //接收中断(接收到的数据必须是0x0d 0x0a结尾)
{
Res =USART_ReceiveData(USART1); //读取接收到的数据
if(MODE){
switch(Res){
case 0x31://前进
GoForward(SPEED,SPEED);
break;
case 0x32://后退
GoBack(SPEED,SPEED);
break;
case 0x33://左转
GoLeft(SPEED,SPEED);
break;
case 0x34://右转
GoRight(SPEED,SPEED);
break;
case 0x35://停止
GoStop();
break;
case 0x36://刷卡
SwipeCard();
delay_ms(3000);//等待三秒
case 0x37://恢复
ResetCard();
delay_ms(3000);//等待三秒
break;
case 0x38://保留
break;
case 0x39://遥控模式切换
GoStop();
MODE=(MODE+1)%2;//0 1 不停切换
break;
}
}else if(MODE==0){//目前是自动模式
if(Res==0x39){
//遥控模式切换
GoStop();
MODE=(MODE+1)%2;//0 1 不停切换
}
}
}
}
#endif
红外传感器驱动
TCRT5000传感器可以发射红外线,当发出的红外线没有被反射或者反射回来的强度不够大,该器件上的光敏三极管关断(理解为没有遇到障碍物),否则反射红外强度较大,遇到障碍物。在黑白线的检测中表现为,黑线吸收大部分的光线,反射强度弱,遇到黑线相当于没有遇到障碍物,该器件输出高电平。
因此为了方便检测高电平,我们需要设置GPIO输入为下拉输入(该模式下:低电平为常态,因而检测高电平更合理)。
根据上面的设计PA4-PA7都可以完成该输入,考虑PA6|PA7可以作为定时器的输出,保留下来,使用PA4-PA5两个引脚作为GPIO的数据输入。
GPIO初始化程序如下:
/*
#作者:LARK
#日期:2023/6/1
#描述:定义循迹的传感器
*/
#include "stm32f10x.h"
#include "stm32f10x_tim.h"
#include "stdint.h"
#include "InfraredSensor.h"
#include "led.h"
extern u16 SPEED;
extern u16 AutoSPEED;
extern u16 SuperSPEED;
void InfraredSensorInit(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //使能PA端口时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD; //下拉输入模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOA.4
GPIO_ResetBits(GPIOA,GPIO_Pin_4);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD; //下拉输入模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOA.5
GPIO_ResetBits(GPIOA,GPIO_Pin_5);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD; //下拉输入模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOA.6
GPIO_ResetBits(GPIOA,GPIO_Pin_6);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD; //下拉输入模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOA.7
GPIO_ResetBits(GPIOA,GPIO_Pin_7);
}
//外部中断4配置与初始化
void EXTIX4_Init(void)
{
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
InfraredSensorInit();
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//使能IO复用时钟
//映射IO和中断线
GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource4);//PA4->Line4
EXTI_InitStructure.EXTI_Line=EXTI_Line4;
EXTI_InitStructure.EXTI_Mode=EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger=EXTI_Trigger_Rising_Falling;//一般情况为低,遇到黑线为高 应该该为上升下降沿都需要中断
EXTI_InitStructure.EXTI_LineCmd=ENABLE;
EXTI_Init(&EXTI_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel=EXTI4_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority=2;
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
//外部中断5
void EXTIX5_Init(void)
{
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
InfraredSensorInit();
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//使能IO复用时钟
//映射IO和中断线
GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource5);//PA4->Line4
EXTI_InitStructure.EXTI_Line=EXTI_Line5;
EXTI_InitStructure.EXTI_Mode=EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger=EXTI_Trigger_Rising_Falling;//一般情况为低,遇到黑线为高
EXTI_InitStructure.EXTI_LineCmd=ENABLE;
EXTI_Init(&EXTI_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel=EXTI9_5_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority=2;
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
//外部中断6
void EXTIX6_Init(void)
{
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
InfraredSensorInit();
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//使能IO复用时钟
//映射IO和中断线
GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource6);//PA6->Line6
EXTI_InitStructure.EXTI_Line=EXTI_Line6;
EXTI_InitStructure.EXTI_Mode=EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger=EXTI_Trigger_Rising_Falling;//一般情况为低,遇到黑线为高
EXTI_InitStructure.EXTI_LineCmd=ENABLE;
EXTI_Init(&EXTI_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel=EXTI9_5_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority=2;
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
//外部中断7
void EXTIX7_Init(void)
{
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
InfraredSensorInit();
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//使能IO复用时钟
//映射IO和中断线
GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource7);//PA7->Line7
EXTI_InitStructure.EXTI_Line=EXTI_Line7;
EXTI_InitStructure.EXTI_Mode=EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger=EXTI_Trigger_Rising_Falling;//一般情况为低,遇到黑线为高
EXTI_InitStructure.EXTI_LineCmd=ENABLE;
EXTI_Init(&EXTI_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel=EXTI9_5_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority=2;
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
extern u16 LeftPin1,RightPin1,LeftPin2,RightPin2;
//书写中断服务函数
void EXTI4_IRQHandler()
{
//对应左1
FollowLine();
EXTI_ClearITPendingBit(EXTI_Line4);
}
void EXTI9_5_IRQHandler()
{
if(EXTI_GetITStatus(EXTI_Line5) != RESET)//对应右1
{
FollowLine();
//执行中断服务函数 即具体要干什么
EXTI_ClearITPendingBit(EXTI_Line5); //中断标志位清除
}
if(EXTI_GetITStatus(EXTI_Line6) != RESET)//对应左2
{
FollowLine();
//执行中断服务函数 即具体要干什么
EXTI_ClearITPendingBit(EXTI_Line6); //中断标志位清除
}
if(EXTI_GetITStatus(EXTI_Line7) != RESET)//对应右2
{
FollowLine();
//执行中断服务函数 即具体要干什么
EXTI_ClearITPendingBit(EXTI_Line7); //中断标志位清除
}
}
外部中断设置
当红外传感器检测到黑线的时候需要触发外部中断从而快速完成方向的控制。
外部中断以及中断服务函数如下:
/*
#作者:LARK
#日期:2023/6/1
#描述:定义循迹的传感器
*/
#include "stm32f10x.h"
#include "stm32f10x_tim.h"
#include "stdint.h"
#include "InfraredSensor.h"
#include "led.h"
extern u16 SPEED;
extern u16 AutoSPEED;
extern u16 SuperSPEED;
void InfraredSensorInit(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //使能PA端口时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD; //下拉输入模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOA.4
GPIO_ResetBits(GPIOA,GPIO_Pin_4);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD; //下拉输入模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOA.5
GPIO_ResetBits(GPIOA,GPIO_Pin_5);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD; //下拉输入模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOA.6
GPIO_ResetBits(GPIOA,GPIO_Pin_6);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD; //下拉输入模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOA.7
GPIO_ResetBits(GPIOA,GPIO_Pin_7);
}
//外部中断4配置与初始化
void EXTIX4_Init(void)
{
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
InfraredSensorInit();
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//使能IO复用时钟
//映射IO和中断线
GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource4);//PA4->Line4
EXTI_InitStructure.EXTI_Line=EXTI_Line4;
EXTI_InitStructure.EXTI_Mode=EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger=EXTI_Trigger_Rising_Falling;//一般情况为低,遇到黑线为高 应该该为上升下降沿都需要中断
EXTI_InitStructure.EXTI_LineCmd=ENABLE;
EXTI_Init(&EXTI_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel=EXTI4_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority=2;
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
//外部中断5
void EXTIX5_Init(void)
{
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
InfraredSensorInit();
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//使能IO复用时钟
//映射IO和中断线
GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource5);//PA4->Line4
EXTI_InitStructure.EXTI_Line=EXTI_Line5;
EXTI_InitStructure.EXTI_Mode=EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger=EXTI_Trigger_Rising_Falling;//一般情况为低,遇到黑线为高
EXTI_InitStructure.EXTI_LineCmd=ENABLE;
EXTI_Init(&EXTI_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel=EXTI9_5_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority=2;
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
//外部中断6
void EXTIX6_Init(void)
{
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
InfraredSensorInit();
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//使能IO复用时钟
//映射IO和中断线
GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource6);//PA6->Line6
EXTI_InitStructure.EXTI_Line=EXTI_Line6;
EXTI_InitStructure.EXTI_Mode=EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger=EXTI_Trigger_Rising_Falling;//一般情况为低,遇到黑线为高
EXTI_InitStructure.EXTI_LineCmd=ENABLE;
EXTI_Init(&EXTI_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel=EXTI9_5_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority=2;
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
//外部中断7
void EXTIX7_Init(void)
{
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
InfraredSensorInit();
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//使能IO复用时钟
//映射IO和中断线
GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource7);//PA7->Line7
EXTI_InitStructure.EXTI_Line=EXTI_Line7;
EXTI_InitStructure.EXTI_Mode=EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger=EXTI_Trigger_Rising_Falling;//一般情况为低,遇到黑线为高
EXTI_InitStructure.EXTI_LineCmd=ENABLE;
EXTI_Init(&EXTI_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel=EXTI9_5_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority=2;
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
extern u16 LeftPin1,RightPin1,LeftPin2,RightPin2;
//书写中断服务函数
void EXTI4_IRQHandler()
{
//对应左1
FollowLine();
EXTI_ClearITPendingBit(EXTI_Line4);
}
void EXTI9_5_IRQHandler()
{
if(EXTI_GetITStatus(EXTI_Line5) != RESET)//对应右1
{
FollowLine();
//执行中断服务函数 即具体要干什么
EXTI_ClearITPendingBit(EXTI_Line5); //中断标志位清除
}
if(EXTI_GetITStatus(EXTI_Line6) != RESET)//对应左2
{
FollowLine();
//执行中断服务函数 即具体要干什么
EXTI_ClearITPendingBit(EXTI_Line6); //中断标志位清除
}
if(EXTI_GetITStatus(EXTI_Line7) != RESET)//对应右2
{
FollowLine();
//执行中断服务函数 即具体要干什么
EXTI_ClearITPendingBit(EXTI_Line7); //中断标志位清除
}
}
主函数
主函数的运行流程如下:
主函数源代码如下:
/* Includes ------------------------------------------------------------------*/
#include "stm32f10x.h"
#include "usart.h"
#include "timer.h"
#include "delay.h"
#include "led.h"
#include <string.h>
#include "MotorDev.h"
#include "SteerDev.h"
#include "InfraredSensor.h"
//主函数
//t = 0.5ms——————-舵机会转动 0 °
//t = 1.0ms——————-舵机会转动 45°
//t = 1.5ms——————-舵机会转动 90°
//t = 2.0ms——————-舵机会转动 135°
//t = 2.5ms——————-舵机会转动180°
extern u16 USART_RX_STA;
u16 LeftPin1=0,RightPin1=0,LeftPin2=0,RightPin2=0;
u16 SPEED=5300;//全局速度设置 速度应该从10设置到190 190为最慢
u8 MODE=1;//模式设置 默认遥控模式
u8 Dir;//小车方向情况
u16 AutoSPEED=5000;//0-7199 0非常快,5000可以正常运行
u16 SuperSPEED=4800;//较快的速度,用在极速的情况下3000较快
int main(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
My_USART1_Init(9600);
LED_Init();
delay_init(); //延时初始化
MotorInit(7199,99);//初始化电机PWM波 对PWM波的频率要求在1K以上20k以下,1k以下无法稳定驱动,20k以上转速过高只有尖刺声
SteerInit(199,7200);//初始化舵机PWM波 初始化周期为20ms
InfraredSensorInit();
EXTIX4_Init();
EXTIX5_Init();
EXTIX6_Init();
EXTIX7_Init();
DirInit();
GoStop();
while(1){
if(MODE==0){
LeftPin1=GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_4);
RightPin1=GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_5);
LeftPin2=GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_6);
RightPin2=GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_7);
if(((LeftPin1||LeftPin2)&&(RightPin1||RightPin2))){
//有两个都检测到,遇到十字路口
//BlinkLED(10);
GoStop();
BlinkLED(10);
SwipeCard();//刷卡
delay_ms(3000);//等待三秒
BlinkLED(10);
ResetCard();//恢复继续往前走
delay_ms(5000);//等待两秒
BlinkLED(10);
GoForward(AutoSPEED,AutoSPEED);
delay_ms(2000);//等待三秒
}
if(LeftPin2&&(!LeftPin1)&&(!RightPin1)&&(!RightPin2)){//只有左2被触发
//稍微偏左
//BlinkLED(1);
GoLeft(AutoSPEED,AutoSPEED);//半速转弯
}
if(RightPin2&&(!RightPin1)&&(!LeftPin1)&&(!LeftPin2)){//只有右2被触发
//稍微偏右
//BlinkLED(2);
GoRight(AutoSPEED,AutoSPEED);//半速转弯
}
if(LeftPin1&&(!LeftPin2)&&(!RightPin1)&&(!RightPin2)){//只有左1被触发
GoLeft(SuperSPEED,SuperSPEED);//全速转弯
while(!RightPin2){
GoLeft(SuperSPEED,SuperSPEED);//全速转弯 直到右边碰到黑线
}
}
if(LeftPin1&&LeftPin2&&(!RightPin1)&&(!RightPin2)){
GoLeft(SuperSPEED,SuperSPEED);//全速转弯
while(!RightPin2){
GoLeft(SuperSPEED,SuperSPEED);//全速转弯 直到右边碰到黑线
}
}
if(RightPin1&&RightPin2&&(!LeftPin1)&&(!LeftPin2)){
GoRight(SuperSPEED,SuperSPEED);//全速转弯
while(!LeftPin2){
GoRight(SuperSPEED,SuperSPEED);//全速转弯直到左边碰到黑线
}
//delay_ms(500);
}
if(RightPin1&&(!RightPin2)&&(!LeftPin1)&&(!LeftPin2)){//只有右1被触发
//非常偏右
//BlinkLED(8);
GoRight(SuperSPEED,SuperSPEED);//全速转弯
while(!LeftPin2){
GoRight(SuperSPEED,SuperSPEED);//全速转弯直到左边碰到黑线
}
}
if((!LeftPin1)&&(!LeftPin2)&&(!RightPin1)&&(!RightPin2)){
//如果全都没有检测到黑线直走
GoForward(AutoSPEED,AutoSPEED);
}
}
}
}
最终效果
小车的最终效果为: