#include <Servo.
h>
// Pin Definitions
const int trigPin = 9;
const int echoPin = 10;
const int leftMotorEnable = 2; // Enable pin for left motor (PWM)
const int leftMotorForward = 3;
const int leftMotorBackward = 4;
const int rightMotorEnable = 7; // Enable pin for right motor (PWM)
const int rightMotorForward = 5;
const int rightMotorBackward = 6;
const int baseServoPin = 8; // Base rotation servo
const int shoulderServoPin = 11; // Shoulder servo
const int clawServoPin = 12; // Claw servo
// Height Levels
const int LOWER_HEIGHT = 120; // Servo angle for lower height
const int HIGHER_HEIGHT = 60; // Servo angle for higher height
// Ultrasonic Variables
float distance = 0.0;
const float detectionThreshold = 10.0; // Object detection threshold in cm
// Servo Instances
Servo baseServo;
Servo shoulderServo;
Servo clawServo;
void setup() {
// Initialize Serial Monitor
[Link](9600);
// Ultrasonic Sensor Setup
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
// Motor Pins Setup
pinMode(leftMotorEnable, OUTPUT);
pinMode(leftMotorForward, OUTPUT);
pinMode(leftMotorBackward, OUTPUT);
pinMode(rightMotorEnable, OUTPUT);
pinMode(rightMotorForward, OUTPUT);
pinMode(rightMotorBackward, OUTPUT);
// Attach Servos
[Link](baseServoPin);
[Link](shoulderServoPin);
[Link](clawServoPin);
// Initialize Servo Positions
[Link](0); // Open claw
[Link](120); // Neutral shoulder position
[Link](30); // Neutral base position
// Initialize H-Bridge Motor Speed (Enable PWM)
analogWrite(leftMotorEnable, 150); // Full speed for left motor
analogWrite(rightMotorEnable, 150); // Full speed for right motor
[Link]("Robot initialized!");
void loop() {
// Detect object at pickup point
distance = measureDistance();
if (distance > 0 && distance < detectionThreshold) {
[Link]("Object detected at pickup point!");
stopMotors();
// Pick the first object and place it at Point 2 (lower height)
pickObject(LOWER_HEIGHT);
moveToPoint(2);
placeObject(LOWER_HEIGHT);
// Return to Point 1 and pick the second object (higher height)
moveToPoint(1);
pickObject(HIGHER_HEIGHT);
moveToPoint(2);
placeObject(HIGHER_HEIGHT);
// Return to initial position
moveToPoint(1);
} else {
moveToPoint(1); // Keep searching at Point 1
delay(100);
// Measure distance using ultrasonic sensor
float measureDistance() {
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
long duration = pulseIn(echoPin, HIGH);
return duration * 0.034 / 2.0; // Convert duration to cm
}
// Move the robot to a specific point
void moveToPoint(int point) {
if (point == 1) {
[Link]("Moving to Point 1...");
driveMotors(HIGH, LOW, HIGH, LOW); // Move forward
delay(3000); // Adjust delay for distance to Point 1
stopMotors();
} else if (point == 2) {
[Link]("Moving to Point 2...");
driveMotors(HIGH, LOW, HIGH, LOW); // Move forward
delay(3000); // Adjust delay for distance to Point 2
stopMotors();
// Stop all motors
void stopMotors() {
driveMotors(LOW, LOW, LOW, LOW);
[Link]("Motors stopped.");
// Pick an object at a specific height
void pickObject(int height) {
[Link]("Picking up object...");
[Link](90); // Open claw
delay(500);
[Link](height); // Move shoulder to the height
delay(1000); // Wait for shoulder to move
[Link](30); // Close claw to grab object
delay(1000);
[Link](90); // Return shoulder to neutral position
delay(1000);
// Place an object at a specific height
void placeObject(int height) {
[Link]("Placing object...");
[Link](height); // Move shoulder to placement height
delay(1000);
[Link](90); // Open claw to release object
delay(1000);
[Link](90); // Return shoulder to neutral position
delay(1000);
// Drive motors using H-bridge
void driveMotors(int leftForward, int leftBackward, int rightForward, int rightBackward) {
digitalWrite(leftMotorForward, leftForward);
digitalWrite(leftMotorBackward, leftBackward);
digitalWrite(rightMotorForward, rightForward);
digitalWrite(rightMotorBackward, rightBackward);