#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;
int distanceKeep = 8;
void setup() {
Serial.begin(115200); // set up Serial library at 115200 bps
Serial.println("robot Follow Object 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(125);
MotorR.setSpeed(125);
// turn off 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 less than 5cm, robot will go backward for 1 second, and turn right for 1 second
if(distanceCm < distanceKeep && distanceCm >= 5){
MotorL.run(BACKWARD);
MotorR.run(BACKWARD);
}
if(distanceCm > (distanceKeep+3) && distanceCm < (distanceKeep+20)){
MotorL.run(FORWARD);
MotorR.run(FORWARD);
}
if(distanceCm > (distanceKeep+20)){
MotorL.run(FORWARD);
MotorR.run(BACKWARD);
}
if(distanceCm >= distanceKeep && distanceCm <= (distanceKeep+3)){
MotorL.run(RELEASE);
MotorR.run(RELEASE);
}
}
//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