This document defines the code for a robot that uses an ultrasonic sensor to detect distance and motors to move. It includes setup of an ultrasonic sensor on pins 3 and 2 to measure distance in cm. It also sets up 4 motors on pins 4-7 and a servo on pin 9. The main loop uses the sensor to stop the motors if an object is closer than 15cm, or moves the motors forward and spins the servo every 5 seconds.

1. #include <Servo.h> //Servo library
Servo servo_test; //Servo setup
int angle = 0; // Servo setup
// motors
int in1 = 4;
int in2 = 5;
int in3 = 6;
int in4 = 7;
unsigned long previousMillis = 0; // will store last time servos were spinning
unsigned long currentMillis = 0;
const long interval = 5000; // interval between each round of spin
int trigPin = 3; // Trigger for ultrasonic sensor
int echoPin = 2; // Echo for ultrasonic sensor
long duration, cm, inches; // ultrasonic sensor setup
void setup() {
Serial.begin (9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
servo_test.attach(9);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
}
void loop() {
// The sensor is triggered by a HIGH pulse of 10 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
digitalWrite(trigPin, LOW);
delayMicroseconds(5);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Read the signal from the sensor: a HIGH pulse whose duration is the time (in microseconds)
// from the sending of the ping to the reception of its echo off of an object.
pinMode(echoPin, INPUT);
duration = pulseIn(echoPin, HIGH);
cm = (duration/2) / 29.1; // Convert time into distance
delay(250);
currentMillis = millis();

2. if (cm < 15) {
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
} else {
// Turn on motors in FORWARD motions
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
if (currentMillis - previousMillis >= interval) {
previousMillis = currentMillis; // save the last time spinning the servo
digitalWrite(in1, LOW); // stop all motors
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
for (angle = 0; angle < 360; angle += 5) { // turn servo 1000 degrees
servo_test.write(angle);
delay(5);
}
delay(400);
for (angle = 360; angle>=1; angle-=5) { // reverse servo back to 0 degrees
servo_test.write(angle);
delay(5);
}
delay(1000);
}
}
}