/ **
* @文件 Arduino UNO自主机器人
* @作者 卡林·德拉戈斯(Calin Dragos)for inrobotics.com
* @版本 V1.0
* @日期 13.10.2016
* @说明 这是能够检测并避开障碍物的自主机器人的Arduino草图
* /
#包括<AFMotor.h>
#包括<Servo.h>
#包括<NewPing.h>
伺服 Myservo;
整型 ENABLE_A = 6;
整型 PIN_A1 = 3;
整型 PIN_A2 = 2;
整型 ENABLE_B = 11;
整型 PIN_B1 = 5;
整型 PIN_B2 = 4;
整型 SENSOR_DISTANCE;
整型 LEFT_SENSOR_DISTANCE;
整型 RIGHT_SENSOR_DISTANCE;
整型 SNZ_DISTANCE_L;
整型 SNZ_DISTANCE_R;
整型 LFT_SNZ_DIS;
整型 RGT_SNZ_DIS;
#定义TRIG_PIN 7
#定义ECHO_PIN 8
#定义MIN_DISTANCE 40
#定义MAX_DISTANCE 200
#定义间隔200
新平 声纳(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
虚空 设定() {
序列号.开始(9600);
//直流电动机的引脚模式
pinMode (ENABLE_A, 输出值);
pinMode (PIN_A1, 输出值);
pinMode (PIN_A2, 输出值);
pinMode (ENABLE_B, 输出值);
pinMode (PIN_B1, 输出值);
pinMode (PIN_B2, 输出值);
//超声波传感器的引脚模式
pinMode(TRIG_PIN, 输出值);
pinMode(ECHO_PIN, 输入);
//用于伺服电机
Myservo.连接(9);
//将超声波传感器设置为居中
传感器中心();
//停止电动机
stopMotors();
}
虚空 循环() {
SENSOR_DISTANCE=sensorDistance();
序列号.打印(“前传感器距离为:”);
序列号.打印(SENSOR_DISTANCE);
如果(SENSOR_DISTANCE >= MIN_DISTANCE || SENSOR_DISTANCE== 0)
{
前进();
序列号.打印(“前进”);
}
其他 {
//停止电动机
stopMotors();
LFT_SNZ_DIS=向左转();
RGT_SNZ_DIS=在右边();
如果(LFT_SNZ_DIS >= MIN_DISTANCE && LFT_SNZ_DIS >= RGT_SNZ_DIS)
{
整型 NEW_SNZ_DIS_LFT;
//尝试三次以逃脱
对于(整型 i=1;i<=3;i++){
往左走();
序列号.打印(“往左走”);
延迟(300);
//停止电动机
stopMotors();
NEW_SNZ_DIS_LFT=sensorDistance();
如果(NEW_SNZ_DIS_LFT >=MIN_DISTANCE)
{
打破;
}
}
传感器中心();
}
其他 如果(RGT_SNZ_DIS >= MIN_DISTANCE && RGT_SNZ_DIS >= LFT_SNZ_DIS)
{
整型 NEW_SNZ_DIS_RGT;
//尝试三次以逃脱
对于(整型 j=1;j<=3;j++){
向右走();
序列号.打印(“向右走”);
延迟(300);
//停止电动机
stopMotors();
NEW_SNZ_DIS_RGT=sensorDistance();
如果(NEW_SNZ_DIS_RGT >=MIN_DISTANCE)
{
打破;
}
}
传感器中心();
}
其他
{
向后走();
序列号.打印(“后退”);
延迟(500);
//停止电动机
stopMotors();
传感器中心();
}
}
}
虚空 stopMotors(){
AnalogWrite (ENABLE_A, 0);
AnalogWrite (ENABLE_B, 0);
digitalWrite (PIN_A1, 低);
digitalWrite (PIN_A2, 低);
digitalWrite (PIN_B1, 低);
digitalWrite (PIN_B2, 低);
}
整型 sensorDistance(){
整型 距离;
整型 我们 = 声纳.ping();
距离 = 我们 / US_ROUNDTRIP_CM;
如果(距离 !=0){
返回 距离;
延迟(300);
}
}
虚空 前进(){
AnalogWrite (ENABLE_A, 255);
digitalWrite (PIN_A1, 高);
digitalWrite (PIN_A2, 低);
AnalogWrite (ENABLE_B, 255);
digitalWrite (PIN_B1, 高);
digitalWrite (PIN_B2, 低);
}
虚空 向后走(){
AnalogWrite (ENABLE_A, 180);
digitalWrite (PIN_A1, 低);
digitalWrite (PIN_A2, 高);
AnalogWrite (ENABLE_B, 180);
digitalWrite (PIN_B1, 低);
digitalWrite (PIN_B2, 高);
}
虚空 往左走(){
AnalogWrite (ENABLE_A, 180);
digitalWrite (PIN_A1, 高);
digitalWrite (PIN_A2, 低);
AnalogWrite (ENABLE_B, 180);
digitalWrite (PIN_B1, 低);
digitalWrite (PIN_B2, 高);
}
虚空 向右走(){
AnalogWrite (ENABLE_A, 180);
digitalWrite (PIN_A1, 低);
digitalWrite (PIN_A2, 高);
AnalogWrite (ENABLE_B, 180);
digitalWrite (PIN_B1, 高);
digitalWrite (PIN_B2, 低);
}
虚空 传感器中心(){
Myservo.写(90);
延迟(500);
}
虚空 turnSensorLeft(){
Myservo.写(120);
延迟(500);
}
虚空 turnSensorRight(){
Myservo.写(60);
延迟(500);
}
整型 leftSensorDistance(){
整型 LEFT_SENSOR_DISTANCE;
LEFT_SENSOR_DISTANCE=sensorDistance();
返回 LEFT_SENSOR_DISTANCE;
}
整型 rightSensorDistance(){
整型 RIGHT_SENSOR_DISTANCE;
RIGHT_SENSOR_DISTANCE=sensorDistance();
返回 RIGHT_SENSOR_DISTANCE;
}
整型 向左转(){
整型 LEFT_SENSOR_DISTANCE;
turnSensorLeft();
LEFT_SENSOR_DISTANCE=leftSensorDistance();
序列号.打印(“左传感器距离为:”);
序列号.打印(LEFT_SENSOR_DISTANCE);
返回 LEFT_SENSOR_DISTANCE;
}
整型 在右边(){
整型 RIGHT_SENSOR_DISTANCE;
turnSensorRight();
RIGHT_SENSOR_DISTANCE=rightSensorDistance();
序列号.打印(“右传感器距离为:”);
序列号.打印(RIGHT_SENSOR_DISTANCE);
返回 RIGHT_SENSOR_DISTANCE;
}