Hi all,
this time I was making a final project entitled automatic trolleys and I had difficulty making collision avoidance programs
I use 3 ultrasonic HC SR04 sensors as a distance meter, 1 servo as a trolley diver with a dead wheel on the front of the trolley, a DC motor in the back of the trolley, and a 2 channel relay module as a DC motor controller for forward and backward
can you help me program?
sorry if my English is bad, I'm from Indonesia
this is my coding
#include <Servo.h>
#include <NewPing.h>
#include "HX711.h"
//#define calibration_factor 48810.10 // nilai hasil Kalibrasi
#define DOUT 30
#define CLK 31
HX711 scale(DOUT, CLK);
float calibration_factor = 507.80;
;
int GRAM;
const int trigPin1 = 4; //depan
const int echoPin1 = 5;
const int trigPin2 = 2; //kiri
const int echoPin2 = 3;
const int trigPin3 = 6; //kanan
const int echoPin3 = 7;
const int ServoPin = 8;
int relay1 = 9; //ban kanan dan kiri maju
int relay2 = 10; //ban kanan dan kiri mundur (kalau mau dipake)
int buzzer = 9;
long durasi1, jarak1; //tipe data untuk menampung bilangan bulat
long durasi2, jarak2;
long durasi3, jarak3;
// 100 = maxJarak
NewPing sonar1 (trigPin1, echoPin1, 100); //trig, mentrigger pemancaran gelombang. echo, menerima gelombang ultrasonik
NewPing sonar2 (trigPin2, echoPin2, 100); //trig, mentrigger pemancaran gelombang. echo, menerima gelombang ultrasonik
NewPing sonar3 (trigPin3, echoPin3, 100); //trig, mentrigger pemancaran gelombang. echo, menerima gelombang ultrasonik
int cm1 = sonar1.ping_cm();
int cm2 = sonar2.ping_cm();
int cm3 = sonar3.ping_cm();
int angle1, angle2, angle3;
Servo sasaServo; //nama servonya adalah Sasa
int Speed_Control = 100;
void setup() {
Serial.begin (9600); //mengaktifkan serial monitor
sasaServo.attach(ServoPin); //merujuk kepada pin 8 Servo
pinMode(trigPin1, OUTPUT); //mendefinisikan control pin sebagai output
pinMode(echoPin1, INPUT); //mendefinisikan control pin sebagai input
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
pinMode(trigPin3, OUTPUT);
pinMode(echoPin3, INPUT);
pinMode(relay1, OUTPUT);
pinMode(relay2, OUTPUT);
pinMode(buzzer, OUTPUT);
digitalWrite(buzzer, LOW);
//sensor berat
scale.set_scale();
scale.tare();
}
void loop() {
depan();
if (relay2 == HIGH){
digitalWrite(relay2, LOW);
digitalWrite(relay1, LOW);
delay(300);
}
delay(500);
//kiri();
//delay(500);
kanan();
delay(100);
Serial.print("Sensor 1 : ");
Serial.print(jarak1);
Serial.println(" cm");
Serial.print("Sensor 2 : ");
Serial.print(jarak2);
Serial.println(" cm");
Serial.print("Sensor 3 : ");
Serial.print(jarak3);
Serial.println(" cm");
}
void depan(int Speed){ //SENSOR DEPAN
//tipe data untuk menampung bilangan bulat
//int cm1 = sonar1.ping_cm();
//int angle1;
digitalWrite(trigPin1, LOW); //tidak mentrigger pemancar gelombang
delayMicroseconds(2); //memberi delay 2 microscnd
digitalWrite(trigPin1, HIGH); //mentrigger pemancar gelombang
delayMicroseconds(10);
digitalWrite(trigPin1, LOW);
durasi1 = pulseIn(echoPin1, HIGH); //durasi adalah pulsa masuk yg dibaca oleh echo menjadi high
jarak1 = (durasi1/2) / 29.1; //kecepatan suara adalah: 343m/s = 0,0343cm/uS = 1 / 29,1cm/uS
if (jarak1 >= 1 && jarak1 <= 21){ //jika jarak sensor depan >=1 dan <=20
Serial.print("Jarak Terlalu Dekat! Troli Mundur."); //maka serial monitor akan mengeluarkan kalimat "Jarak Terlalu Dekat!"
digitalWrite(buzzer, LOW); //kemudian buzzer menyala
digitalWrite(relay1, LOW); //motor kanan dan kiri mati (maju)
digitalWrite(relay2, HIGH); //motor kanan dan kiri nyala (mundur)
}
else if (jarak1 >= 25 && jarak1 <= 60){
Serial.print("Troli Maju!"); //troli maju
digitalWrite(relay1, HIGH);
digitalWrite(relay2, LOW);
}
else { //selain di atas
Serial.print("Troli Berhenti");
digitalWrite(buzzer, HIGH); //buzzer mati
digitalWrite(relay1, LOW); //motor kanan dan kiri nyala (maju)
digitalWrite(relay2, LOW); //motor kanan dan kiri mati (mundur)
}
}
//Sensor Berat
/scale.set_scale(calibration_factor);
GRAM = scale.get_units(), 4;
Serial.print("berat troli : ");
Serial.print(GRAM);
Serial.println(" Gram");
delay(500);/
void kiri(){ //SENSOR KIRI - PING 2
int cm2 = sonar2.ping_cm(); //mendapatkan hasil cm
int angle2;
//int angle = map(cm, 2, 15, 0, 90); //value, fromLow, formHigh, toLow, toHigh
//sasaServo.write(angle); //membaca perputaran derajat
digitalWrite(trigPin2, LOW);
delayMicroseconds(2);
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin2, LOW);
durasi2 = pulseIn(echoPin2, HIGH);
jarak2 = (durasi2/2) / 29.1;
if (jarak2 >= 1 && jarak2 <= 10){ //jika jarak sensor ping 2 lebih dari atau sama dengan 50 dan kurang dari atau sama dengan 0 (karena jangkauan deteksi ultrasonic sampai kisaran 200-500cm)
angle2 = map(cm2, 5, 15, 90, 100);
sasaServo.write(angle2);
}
else {
sasaServo.write(90);
}
}
void kanan(){ //SENSOR KANAN - PING 3
int cm3 = sonar3.ping_cm(); //mendapatkan hasil cm
int angle3;
//int angle2 = map(cm2, 2, 15, 0, 90); //value, fromLow, formHigh, toLow, toHigh
//sasaServo.write(angle2); //membaca perputaran derajat
digitalWrite(trigPin3, LOW);
delayMicroseconds(2);
digitalWrite(trigPin3, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin3, LOW);
durasi3 = pulseIn(echoPin3, HIGH);
jarak3 = (durasi3/2) / 29.1;
if (jarak3 >= 1 && jarak3 <= 10){ //jika jarak sensor kanan >=5 dan <=10 maka troli berputar 45 derajat
//angle3 = map(cm3, 2, 10, 90, 0);
sasaServo.write(40);
}
else if (jarak3 <= 10 && jarak1 <= 18){
sasaServo.write(35);
}
else {
sasaServo.write(90);
}
}