单发单收超声波跟随小车程序
单片机源程序如下:
- const int IN1 = 20;//定义控制IO口“D20”
- const int IN2 = 21;//定义控制IO口“D21”
- const int IN3 = 22;//定义控制IO口“D22”
- const int IN4 = 23;//定义控制IO口“D23”
- const int ENA = 3; //定义控制PWM输出口“3”
- const int ENB = 4; //定义控制PWM输出口“4”
- const int Bled = 13; //背光灯控制脚定义D13
- const int buzzer= 12; //蜂鸣器控制脚定义D9
- const int voltPin = A3; //定义电压检测接口A5模拟口。
- const int LSPPin = A7; //定义电压检测接口A5模拟口。
- const int RSPPin = A6; //定义电压检测接口A5模拟口。
- const int FSPPin = A5; //定义电压检测接口A5模拟口。
- int LSP=0,RSP=0,LRSP=0,FSP=0;
- int chazhi=300;
- void setup() {
- pinMode(IN1, OUTPUT);//定义控制IO口为输出“D2”
- pinMode(IN2, OUTPUT);//定义控制IO口为输出“D3”
- pinMode(IN3, OUTPUT);//定义控制IO口为输出“D4”
- pinMode(IN4, OUTPUT);//定义控制IO口为输出“D5”
- pinMode(ENA, OUTPUT);//定义控制IO口为输出“D4”
- pinMode(ENB, OUTPUT);//定义控制IO口为输出“D5”
- pinMode(Bled, OUTPUT); //LCD背光灯控制口设置为输出
- pinMode(buzzer, OUTPUT); // 将蜂鸣器引脚9设置为输出
- Serial.begin(9600);
- }
- void loop() {
- FSP = analogRead(FSPPin);
- delay(1);
- LSP = analogRead(LSPPin);
- delay(1);
- RSP = analogRead(RSPPin);
- delay(1);
- LRSP = abs(LSP - RSP);
- Serial.print("robot=");
- Serial.print(FSP);
- Serial.print(",");
- Serial.print(LSP);
- Serial.print(",");
- Serial.print(RSP);
- Serial.print(",");
- Serial.println(LRSP);
- if (LSP > RSP && LRSP > chazhi) {
- zuo(255);
- }
- if (LSP < RSP && LRSP > chazhi) {
- you(255);
- }
- if(FSP > 20 && LRSP < 200){
- qian(255);
- } else
- if(FSP <20){
- ting();
- }
- }
- void qian(int xo)//定义前进带参数函数
- {
- digitalWrite(IN1,HIGH); //定义D2输出高电平
- digitalWrite(IN2,LOW); //定义D3输出低电平
- digitalWrite(IN3,HIGH); //定义D4输出高电平
- digitalWrite(IN4,LOW); //定义D5输出低电平
- analogWrite(ENA,xo);//定义D6输出PWM
- analogWrite(ENB,xo);//定义D7输出PWM
- }
- void hou(int xo)//定义后退带参数函数
- {
- digitalWrite(IN1,LOW); //定义D2输出低电平
- digitalWrite(IN2,HIGH); //定义D3输出高电平
- digitalWrite(IN3,LOW); //定义D4输出低电平
- digitalWrite(IN4,HIGH); //定义D5输出高电平
- analogWrite(ENA,xo);//定义D6输出PWM
- analogWrite(ENB,xo);//定义D7输出PWM
- }
- void zuo(int xo)//定义左转带参数函数
- {
- digitalWrite(IN1,LOW); //定义D2输出低电平
- digitalWrite(IN2,HIGH); //定义D3输出高电平
- digitalWrite(IN3,HIGH); //定义D4输出高电平
- digitalWrite(IN4,LOW); //定义D5输出低电平
- analogWrite(ENA,xo);//定义D6输出PWM
- analogWrite(ENB,xo);//定义D7输出PWM
- }
- void you(int xo)//定义右转带参数函数
- {
- digitalWrite(IN1,HIGH); //定义D2输出高电平
- digitalWrite(IN2,LOW); //定义D3输出低电平
- digitalWrite(IN3,LOW); //定义D4输出低电平
- digitalWrite(IN4,HIGH); //定义D5输出高电平
- analogWrite(ENA,xo);//定义D6输出PWM
- analogWrite(ENB,xo);//定义D7输出PWM
- }
- void ting()//定义停止带参数函数
- {
- digitalWrite(IN1,LOW); //定义D2输出低电平
- digitalWrite(IN2,LOW); //定义D3输出低电平
- digitalWrite(IN3,LOW); //定义D4输出低电平
- digitalWrite(IN4,LOW); //定义D5输出低电平
- digitalWrite(ENA,LOW);//定义D6输出PWM
- digitalWrite(ENB,LOW);//定义D7输出PWM
- }
复制代码
所有资料51hei提供下载:
超声波追踪代码.rar
(1.13 KB, 下载次数: 43)
|