#include <AFMotor.h>
AF_DCMotor MotorL(4); // Motor for drive Left on M4
AF_DCMotor MotorR(3); // Motor for drive Right on M3
//ultrasonic setup:
const int trigPin = A4; // trig pin connected to Arduino's pin A4
const int echoPin = A5; // echo pin connected to Arduino's pin A5
// defines variables
long duration;
int distanceCm=0;
void setup() {
Serial.begin(115200); // set up Serial library at 115200 bps
Serial.println("*robot Edge Avoidance Mod*");
pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
pinMode(echoPin, INPUT); // Sets the echoPin as an Input
// Set the speed to start, from 0 (off) to 255 (max speed)
// sometimes the motors don't have the same speed, so use these values tomake your robot move straight
MotorL.setSpeed(100);
MotorR.setSpeed(100);
// turn on motor
MotorL.run(RELEASE);
MotorR.run(RELEASE);
}
// main program loop
void loop() {
distanceCm=getDistance(); // variable to store the distance measured by the sensor
Serial.println(distanceCm); // print the distance that was measured
//if the distance is more than 10cm, robot will go backward for 1 second, and turn right for 1 second
if(distanceCm > 7){
MotorL.run(BACKWARD);
MotorR.run(BACKWARD);
delay(500);
MotorL.run(FORWARD);
MotorR.run(BACKWARD);
delay(500);
} else {
MotorL.run(FORWARD); //otherwise it will continue forward
MotorR.run(FORWARD);
}
}
//RETURNS THE DISTANCE MEASURED BY THE HC-SR04 DISTANCE SENSOR
int getDistance() {
int echoTime; //variable to store the time it takes for a ping to bounce off an object
int calcualtedDistance; //variable to store the distance calculated from the echo time
//send out an ultrasonic pulse that's 10ms long
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
echoTime = pulseIn(echoPin, HIGH); //use the pulsein command to see how long it takes for the
//pulse to bounce back to the sensor
calcualtedDistance = echoTime / 58.26; //calculate the distance of the object that reflected the pulse (half the bounce time multiplied by the speed of sound)
return calcualtedDistance; //send back the distance that was calculated
}
Comments