No edit summary |
No edit summary |
||
| Line 76: | Line 76: | ||
Below: The Code for controlling the motor's speed and direction, via the ultrasonic sensor. The sensor outputs a range of distance values, and by continuously detecting the distance it can predict the mode of visitor's movement. | Below: The Code for controlling the motor's speed and direction, via the ultrasonic sensor. The sensor outputs a range of distance values, and by continuously detecting the distance it can predict the mode of visitor's movement. | ||
int SNUM[3] ;//定义3个传感器，从SNUM[0]-SNUM[2]为从左到右3个红外避障传感器 | int SNUM[3] ;//定义3个传感器，从SNUM[0]-SNUM[2]为从左到右3个红外避障传感器 | ||
int INA = 4; //电机A正反转控制端 | int INA = 4; //电机A正反转控制端 | ||
int PWMA = 5; //电机A调速端 | int PWMA = 5; //电机A调速端 | ||
| Line 95: | Line 95: | ||
analogWrite(PWMB,abs (sp2)); | analogWrite(PWMB,abs (sp2)); | ||
} | } | ||
void setup(){ | void setup(){ | ||
pinMode(11, INPUT); //配置左传感器IO口为输入 | pinMode(11, INPUT); //配置左传感器IO口为输入 | ||
pinMode(12, INPUT); //配置中传感器IO口为输入 | pinMode(12, INPUT); //配置中传感器IO口为输入 | ||
| Line 103: | Line 102: | ||
pinMode(INA,OUTPUT); | pinMode(INA,OUTPUT); | ||
pinMode(INB,OUTPUT); //配置电机驱动IO口为输出 | pinMode(INB,OUTPUT); //配置电机驱动IO口为输出 | ||
} | |||
} | void loop(){ | ||
void loop(){ | SNUM[0] = digitalRead(11);//左传感器赋值 | ||
SNUM[0] = digitalRead(11);//左传感器赋值 | |||
SNUM[1] = digitalRead(12); //中传感器赋值 | SNUM[1] = digitalRead(12); //中传感器赋值 | ||
SNUM[2] = digitalRead(13); //右传感器赋值 | SNUM[2] = digitalRead(13); //右传感器赋值 | ||
| Line 116: | Line 113: | ||
Serial.println(SNUM[2]); | Serial.println(SNUM[2]); | ||
if ((SNUM[0]==1)&&(SNUM[1]==1)&&(SNUM[2]==1))//所有传感器都没有检测到障碍 | if ((SNUM[0]==1)&&(SNUM[1]==1)&&(SNUM[2]==1))//所有传感器都没有检测到障碍 | ||
motospd(100,100); //直行 | motospd(100,100); //直行 | ||
if ((SNUM[0]==0)&&(SNUM[1]==1)&&(SNUM[2]==1)) //左传感器检测到障碍物 | if ((SNUM[0]==0)&&(SNUM[1]==1)&&(SNUM[2]==1)) //左传感器检测到障碍物 | ||
motospd(100,50); //右转 | motospd(100,50); //右转 | ||
| Line 123: | Line 120: | ||
if ((SNUM[0]==1)&&(SNUM[1]==0)&&(SNUM[2]==1)) //中传感器检测到障碍物 | if ((SNUM[0]==1)&&(SNUM[1]==0)&&(SNUM[2]==1)) //中传感器检测到障碍物 | ||
motospd(-100,-100); //后退 | motospd(-100,-100); //后退 | ||
delay(50); | delay(50);} | ||
} | |||
const int pingPin = 7; // Trigger Pin of Ultrasonic Sensor | const int pingPin = 7; // Trigger Pin of Ultrasonic Sensor | ||
const int echoPin = 6; // Echo Pin of Ultrasonic Sensor | const int echoPin = 6; // Echo Pin of Ultrasonic Sensor | ||
void setup() { | void setup() { | ||
Serial.begin(9600); // Starting Serial Terminal | Serial.begin(9600); // Starting Serial Terminal | ||
} | } | ||
void loop() { | void loop() { | ||
long duration, inches, cm; | long duration, inches, cm; | ||
pinMode(pingPin, OUTPUT); | pinMode(pingPin, OUTPUT); | ||