Arduino 沿墙机器人代码:自动避障并停止
// 定义引脚
int red_led = 11; // 红色LED灯引脚
volatile int proximity[8]; // 超声波传感器数组
unsigned char analogPin[8] = {A0,A1,A2,A3,A4,A5,A6,A7}; // 超声波传感器模拟引脚
int PWM_left = 6; // 左电机PWM引脚
int left_dir = 7; // 左电机方向引脚
int PWM_right = 5; // 右电机PWM引脚
int right_dir = 2; // 右电机方向引脚
volatile int base_speed = 12; // 基础速度
void setup() {
// 初始化设置
pinMode(red_led, OUTPUT); // 设置红色LED为输出
Serial.begin(57600); // 初始化串口通信
}
void loop() {
// 主循环
// 读取传感器数值
// proximity sensor5/6/7感应到左侧存在墙体则开始按上述代码进行沿左侧墙运动
if (digitalRead(5) == HIGH || digitalRead(6) == HIGH || digitalRead(7) == HIGH) {
followWall();
}
// 当proximity sensor0感应到前方存在障碍物且proximity sensor2感应到右边不存在障碍物时,右转
else if (digitalRead(0) == HIGH && digitalRead(2) == LOW) {
turnRight();
}
// 当四个地面传感器同时感应到黑色时停止
else if (digitalRead(3) == LOW && digitalRead(4) == LOW && digitalRead(8) == LOW && digitalRead(9) == LOW) {
MotorStop();
}
// 继续沿左侧墙做沿墙运动
else {
followWall();
}
}
// 沿左侧墙运动
void followWall() {
int distance1 = digitalRead(7);
int distance2 = digitalRead(5);
Serial.print("distance1:");
Serial.println(distance1);
Serial.print("distance2:");
Serial.println(distance2);
if (distance1 == HIGH && distance2 == HIGH) {
leftMotorForward(base_speed + 2);
rightMotorForward(base_speed);
}
else if (distance1 == HIGH && distance2 == LOW) {
leftMotorForward(base_speed);
rightMotorForward(base_speed + 1);
}
else if (distance1 == LOW && distance2 == LOW) {
leftMotorForward(base_speed + 5);
rightMotorForward(base_speed);
}
}
// 右转
void turnRight() {
leftMotorForward(base_speed + 2);
rightMotorBackward(base_speed);
delay(1000);
}
// 停止
void MotorStop() {
digitalWrite(right_dir, HIGH);
digitalWrite(PWM_right, HIGH);
digitalWrite(left_dir, HIGH);
digitalWrite(PWM_left, HIGH);
}
// 左轮前进
void leftMotorForward(float duty) {
int dutyInt = duty;
int dutyVal = map(dutyInt, 0, 100, 0, 100);
analogWrite(PWM_left, dutyVal);
analogWrite(left_dir, 0);
}
// 右轮前进
void rightMotorForward(float duty) {
int dutyInt = duty;
int dutyVal = map(dutyInt, 0, 100, 0, 100);
analogWrite(PWM_right, dutyVal);
analogWrite(right_dir, 0);
}
// 左轮后退
void leftMotorBackward(float duty) {
int dutyInt = duty;
int dutyVal = map(dutyInt, 0, 100, 0, 80);
analogWrite(PWM_left, 0);
analogWrite(left_dir, dutyVal);
}
// 右轮后退
void rightMotorBackward(float duty) {
int dutyInt = duty;
int dutyVal = map(dutyInt, 0, 100, 0, 80);
analogWrite(PWM_right, 0);
analogWrite(right_dir, dutyVal);
}
原文地址: https://www.cveoy.top/t/topic/jpUp 著作权归作者所有。请勿转载和采集!