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;
11const int LEFT = A3;
12const int TRIGGER_PIN = A1;
13const int ECHO_PIN = A0;
14const int MAX_DISTANCE = 200;
15
16
17unsigned int distance = 0;
18unsigned int Right_Value = 0;
19unsigned int Left_Value = 0;
20
21
22NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_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);
40 pinMode(LEFT, 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);
55 distance = sonar.ping_cm();
56 Serial.print("Distance: ");
57 Serial.println(distance);
58 Right_Value = digitalRead(RIGHT);
59 Left_Value = digitalRead(LEFT);
60
61
62 Serial.print("RIGHT: ");
63 Serial.println(Right_Value);
64 Serial.print("LEFT: ");
65 Serial.println(Left_Value);
66
67
68 if ((distance > 15) && (distance < 25)) {
69
70 Serial.println("Move Forward");
71 PWM.setPWM(servo2, 0, (Servo2Degree += 3));
72
73
74 } else if ((Right_Value == 0) && (Left_Value == 0)) {
75
76 Serial.println("Move Right");
77 PWM.setPWM(servo1, 0, Servo1Degree += 3);
78
79
80 } else if ((Right_Value == 1) && (Left_Value == 1)) {
81
82 Serial.println("Move Left");
83 PWM.setPWM(servo1, 0, Servo1Degree -= 3);
84
85
86 } else if ((distance > 5) && (distance < 15)) {
87
88 Serial.println("Move Backward");
89 PWM.setPWM(servo2, 0, Servo2Degree -= 3);
90
91
92 } else if ((distance > 25) && (distance < 35)) {
93
94 Serial.println("Move Downward");
95 PWM.setPWM(servo3, 0, Servo3Degree -= 3);
96 } else if ((distance > 35) && (distance < 45)) {
97
98 Serial.println("Move Upward");
99 PWM.setPWM(servo3, 0, Servo3Degree += 3);
100
101
102 } else if ((distance > 1) && (distance <= 5)) {
103
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
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}
bob-easyeda
22 days ago
Hi, your project is incredible! We’re offering up to $5000 in coupons for fabrication costs via OSHWLab Stars: https://oshwlab.com/activities/spark2023. Plus, we have cool programs to win PCB coupons, like Campus Ambassadors, Video Creators, and Collaboration Sponsorships. Interested in joining? Let’s chat!