// Robotics program for use on Instructables. Thanks for All Emanuel Cesar ( Brasil ) ... Public Domain ! #define trigPin 7 #define echoPin 6 #define ledPin 4 #define led2Pin 5 #include Servo myservo; // create servo object to control a servo int pos = 0; int pinI1 = 8; //define I1 interface determine Motor Direction int pinI2 = 11; //define I2 interface determine Motor Direction int speedpinA = 9; //enable motor A & speed control int pinI3 = 12; //define I3 interface determine Motor Direction int pinI4 = 13; //define I4 interface determine Motor Direction int speedpinB = 10; //enable motor B & speed control int led = 4; // Geen Led int led2 = 5 ; // Red Led void setup() { Serial.begin (9600); pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT); pinMode(led, OUTPUT);// pinMode(led2, OUTPUT);// pinMode(pinI1, OUTPUT); pinMode(pinI2, OUTPUT); pinMode(speedpinA, OUTPUT); pinMode(pinI3, OUTPUT); pinMode(pinI4, OUTPUT); pinMode(speedpinB, OUTPUT); } void takeaction() { myservo.attach(03); // attaches the servo on pin 3 to the servo object } void loop() { long duration, distance; digitalWrite(trigPin, LOW); // Sonar action delayMicroseconds(2); // Added this line digitalWrite(trigPin, HIGH); // Sonar action delayMicroseconds(10); duration = pulseIn(echoPin, HIGH); // Sonar action distance = (duration / 2) / 29.1; if (distance < 02 ) { // LED On/Off Actions analogWrite(speedpinA, 240); //Seep Up of motors to 240 analogWrite(speedpinB, 240); //Seep Up of motors to 240 digitalWrite(pinI4, LOW); // determine Motor B Direction digitalWrite(pinI3, HIGH); digitalWrite(pinI2, LOW); // determine Motor A Direction digitalWrite(pinI1, HIGH); digitalWrite(led, LOW); // Forward ( Green ) Led Off digitalWrite(led2, HIGH); // Backward ( Red ) Led On . } else if (distance > 10 ) { analogWrite(speedpinA, 240); //Drive Forward analogWrite(speedpinB, 240); //Drive Forward digitalWrite(pinI4, LOW); //determine Motor B Direction digitalWrite(pinI3, HIGH); digitalWrite(pinI2, LOW); //determine Motor A Direction digitalWrite(pinI1, HIGH); digitalWrite(led, LOW); // Forward ( Green ) Led Off digitalWrite(led2, HIGH); // Backward ( Red ) Led On . } if (distance >= 30 || distance <= 0) { analogWrite(speedpinA, 240); // Drive Forward analogWrite(speedpinB, 240); // Drive Forward digitalWrite(pinI4, LOW); // determine Motor B Direction digitalWrite(pinI3, HIGH); digitalWrite(pinI2, HIGH); // determine Motor A Direction digitalWrite(pinI1, LOW); digitalWrite(led2, LOW); // Backward ( Red ) Led Off digitalWrite(led, HIGH); // Forward ( Green ) Led On } else if (distance > 45 ) { analogWrite(speedpinA, 130); //Drive Forward ...reducing Motor speeo to 130 analogWrite(speedpinB, 130); //Drive Forward..reducing Motor speeo to 130 digitalWrite(pinI4, LOW); // determine Motor B Direction digitalWrite(pinI3, HIGH); digitalWrite(pinI2, HIGH); // determine Motor A Direction digitalWrite(pinI1, LOW); digitalWrite(led,HIGH ); // Forward ( Green ) Led On digitalWrite(led2, LOW ); // Backward ( Red ) Led Off } }