Build Your Own Object Tracking 4-DOF Robotics Arm With Arduino

1#include <Wire.h>
2#include <Adafruit_PWMServoDriver.h>
3#include <SoftwareSerial.h>
4#include <NewPing.h>
5
6
7Adafruit_PWMServoDriver PWM = Adafruit_PWMServoDriver();
8
9
10const int RIGHT = A2;               // Right IR sensor connected to analog pin A2 of Arduino Uno
11const int LEFT = A3;                // Left IR sensor connected to analog pin A3 of Arduino Uno
12const int TRIGGER_PIN = A1;         // Trigger pin connected to analog pin A1 of Arduino Uno
13const int ECHO_PIN = A0;            // Echo pin connected to analog pin A0 of Arduino Uno
14const int MAX_DISTANCE = 200;       // Maximum ping distance
15
16
17unsigned int distance = 0;          // Variable to store ultrasonic sensor distance
18unsigned int Right_Value = 0;       // Variable to store Right IR sensor value
19unsigned int Left_Value = 0;        // Variable to store Left IR sensor value
20
21
22NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);  // NewPing setup of pins and maximum distance
23
24
25const int servo1 = 0;
26const int servo2 = 1;
27const int servo3 = 2;
28const int servo4 = 3;
29
30
31int Servo1Degree = 150;
32int Servo2Degree = 150;
33int Servo3Degree = 150;
34int Servo4Degree = 325;
35
36
37void setup() {
38  Serial.begin(9600);
39  pinMode(RIGHT, INPUT); // Set analog pin RIGHT as an input
40  pinMode(LEFT, INPUT);  // Set analog pin LEFT as an input
41
42
43  PWM.begin();
44  PWM.setPWMFreq(60);
45  PWM.setPWM(servo1, 0, Servo1Degree);
46  PWM.setPWM(servo2, 0, Servo2Degree);
47  PWM.setPWM(servo3, 0, Servo3Degree);
48  PWM.setPWM(servo4, 0, Servo4Degree);
49  delay(3000);
50}
51
52
53void loop() {
54  delay(50);                                      // Wait 50ms between pings
55  distance = sonar.ping_cm();                     // Send ping, get distance in cm and store it in 'distance' variable
56  Serial.print("Distance: ");
57  Serial.println(distance);                       // Print the distance in the serial monitor
58  Right_Value = digitalRead(RIGHT);               // Read the value from Right IR sensor
59  Left_Value = digitalRead(LEFT);                 // Read the value from Left IR sensor
60
61
62  Serial.print("RIGHT: ");
63  Serial.println(Right_Value);                    // Print the right IR sensor value in the serial monitor
64  Serial.print("LEFT: ");
65  Serial.println(Left_Value);                     // Print the left IR sensor value in the serial monitor
66
67
68  if ((distance > 15) && (distance < 25)) {        // Check whether the ultrasonic sensor's value stays between 15 to 25.
69    // Move Forward:
70    Serial.println("Move Forward");
71    PWM.setPWM(servo2, 0, (Servo2Degree += 3));
72
73
74  } else if ((Right_Value == 0) && (Left_Value == 0)) {
75    // Move Right
76    Serial.println("Move Right");
77    PWM.setPWM(servo1, 0, Servo1Degree += 3);
78
79
80  } else if ((Right_Value == 1) && (Left_Value == 1)) {
81    // Move Left
82    Serial.println("Move Left");
83    PWM.setPWM(servo1, 0, Servo1Degree -= 3);
84
85
86  } else if ((distance > 5) && (distance < 15)) {
87    // Move Backward
88    Serial.println("Move Backward");
89    PWM.setPWM(servo2, 0, Servo2Degree -= 3);
90
91
92  } else if ((distance > 25) && (distance < 35)) {
93    // Move Downward
94    Serial.println("Move Downward");
95    PWM.setPWM(servo3, 0, Servo3Degree -= 3);
96  } else if ((distance > 35) && (distance < 45)) {
97    // Move Upward
98    Serial.println("Move Upward");
99    PWM.setPWM(servo3, 0, Servo3Degree += 3);
100
101
102  } else if ((distance > 1) && (distance <= 5)) {
103    // Open Finger
104    PWM.setPWM(servo4, 0, 325);
105    delay(150);
106    PWM.setPWM(servo4, 0, 400);
107    delay(900);
108    PWM.setPWM(servo4, 0, 325);
109
110
111  } else if ((distance > 40) && (Right_Value == 1) && (Left_Value == 0)) {
112    // Move Stop
113    Serial.println("Move Stop");
114    PWM.setPWM(servo1, 0, Servo1Degree);
115    PWM.setPWM(servo2, 0, Servo2Degree);
116    PWM.setPWM(servo3, 0, Servo3Degree);
117    PWM.setPWM(servo4, 0, 325);
118  }
119}