905 lines
17 KiB
C++
905 lines
17 KiB
C++
#include <WiFi.h>
|
|
#include <HTTPClient.h>
|
|
#include <ESP32Servo.h>
|
|
#include "HX711.h"
|
|
|
|
// =====================================================
|
|
// ================= WIFI ==============================
|
|
// =====================================================
|
|
|
|
const char* ssid = "Fridho";
|
|
const char* password = "123456789.";
|
|
|
|
String serverURL = "http://10.10.1.112:5000";
|
|
String camURL = "http://192.168.137.224";
|
|
|
|
// =====================================================
|
|
// ================= MOTOR =============================
|
|
// =====================================================
|
|
|
|
#define IN1 12
|
|
#define IN2 13
|
|
#define ENA 14
|
|
|
|
#define IR_START 5
|
|
#define IR_STOP 4
|
|
|
|
#define PWM_CHANNEL 7
|
|
|
|
bool motorOn = false;
|
|
bool lastStart = HIGH;
|
|
bool lastStop = HIGH;
|
|
|
|
int speedMotor = 200;
|
|
|
|
// =====================================================
|
|
// ================= LOADCELL ==========================
|
|
// =====================================================
|
|
|
|
#define DOUT 18
|
|
#define CLK 19
|
|
|
|
HX711 scale;
|
|
|
|
float calibration_factor = 942.0;
|
|
float stableWeight = 0;
|
|
|
|
// =====================================================
|
|
// ================= ROBOT ARM =========================
|
|
// =====================================================
|
|
|
|
Servo servo1, servo2, servo3, servo4, servo5, servo6;
|
|
|
|
#define SERVO1_PIN 25
|
|
#define SERVO2_PIN 26
|
|
#define SERVO3_PIN 27
|
|
#define SERVO4_PIN 33
|
|
#define SERVO5_PIN 22
|
|
#define SERVO6_PIN 21
|
|
|
|
int currentPos[6] = {137, 160, 90, 70, 55, 90};
|
|
|
|
// =====================================================
|
|
// ================= SERVO =============================
|
|
// =====================================================
|
|
|
|
void writeServo(int num, int ang) {
|
|
|
|
switch (num) {
|
|
|
|
case 0: servo1.write(ang); break;
|
|
case 1: servo2.write(ang); break;
|
|
case 2: servo3.write(ang); break;
|
|
case 3: servo4.write(ang); break;
|
|
case 4: servo5.write(ang); break;
|
|
case 5: servo6.write(ang); break;
|
|
}
|
|
}
|
|
|
|
float easing(float t) {
|
|
|
|
return t * t * t * (t * (6 * t - 15) + 10);
|
|
}
|
|
|
|
// =====================================================
|
|
// ================= MOVE ALL ==========================
|
|
// =====================================================
|
|
|
|
void moveAll(int target[6], int duration) {
|
|
|
|
int start[6];
|
|
|
|
for (int i = 0; i < 6; i++) {
|
|
start[i] = currentPos[i];
|
|
}
|
|
|
|
int steps = duration / 20;
|
|
|
|
if (steps < 1) steps = 1;
|
|
|
|
for (int s = 0; s <= steps; s++) {
|
|
|
|
float t = easing((float)s / steps);
|
|
|
|
for (int i = 0; i < 6; i++) {
|
|
|
|
int pos = start[i] + (target[i] - start[i]) * t;
|
|
|
|
writeServo(i, pos);
|
|
|
|
currentPos[i] = pos;
|
|
}
|
|
|
|
delay(20);
|
|
}
|
|
}
|
|
|
|
// =====================================================
|
|
// ================= MOVE SEQUENTIAL ===================
|
|
// =====================================================
|
|
|
|
void moveSequential(int target[6]) {
|
|
|
|
for (int i = 1; i <= 5; i++) {
|
|
|
|
int temp[6];
|
|
|
|
for (int j = 0; j < 6; j++) {
|
|
temp[j] = currentPos[j];
|
|
}
|
|
|
|
temp[i] = target[i];
|
|
|
|
moveAll(temp, 500);
|
|
}
|
|
|
|
int temp[6];
|
|
|
|
for (int j = 0; j < 6; j++) {
|
|
temp[j] = currentPos[j];
|
|
}
|
|
|
|
temp[0] = target[0];
|
|
|
|
moveAll(temp, 800);
|
|
}
|
|
|
|
// =====================================================
|
|
// ================= POSISI ============================
|
|
// =====================================================
|
|
|
|
void posisiAwal() {
|
|
|
|
int t[6] = {137, 160, 90, 70, 55, 90};
|
|
|
|
moveAll(t, 1200);
|
|
}
|
|
|
|
void posisiMati() {
|
|
|
|
int t[6] = {137, 160, 60, 70, 55, 90};
|
|
|
|
moveAll(t, 1200);
|
|
}
|
|
|
|
void posisiAntara() {
|
|
|
|
int t[6] = {137,170,75,85,55,90};
|
|
|
|
moveSequential(t);
|
|
}
|
|
|
|
// =====================================================
|
|
// ================= PROSES ROBOT ======================
|
|
// =====================================================
|
|
|
|
void prosesRobot(char g) {
|
|
|
|
unsigned long tStart = millis();
|
|
|
|
posisiAwal();
|
|
|
|
delay(300);
|
|
|
|
// =================================================
|
|
// POSISI AMBIL
|
|
// =================================================
|
|
|
|
int ambil_open[6] = {137,165,40,95,55,90};
|
|
|
|
moveAll(ambil_open, 1200);
|
|
|
|
delay(300);
|
|
|
|
// =================================================
|
|
// CAPIT TELUR
|
|
// =================================================
|
|
|
|
int servo4Grip = 95;
|
|
|
|
// KHUSUS TL LEBIH NUNDUK
|
|
if (g == 'T') {
|
|
servo4Grip = 100;
|
|
}
|
|
|
|
int grip[6] = {
|
|
137,
|
|
165,
|
|
40,
|
|
servo4Grip,
|
|
35,
|
|
110
|
|
};
|
|
|
|
moveAll(grip, 700);
|
|
|
|
// =================================================
|
|
// ANGKAT TELUR
|
|
// =================================================
|
|
|
|
int angkat[6] = {137,170,75,85,35,110};
|
|
|
|
moveAll(angkat, 1200);
|
|
|
|
delay(300);
|
|
|
|
unsigned long tAmbil = millis();
|
|
|
|
// =================================================
|
|
// GERAK KE GRADE
|
|
// =================================================
|
|
|
|
int baseMove[6];
|
|
|
|
for (int i = 0; i < 6; i++) {
|
|
baseMove[i] = currentPos[i];
|
|
}
|
|
|
|
if (g == 'A') baseMove[0] = 80;
|
|
else if (g == 'B') baseMove[0] = 50;
|
|
else if (g == 'C') baseMove[0] = 10;
|
|
else if (g == 'T') baseMove[0] = 180;
|
|
|
|
int selisih = abs(currentPos[0] - baseMove[0]);
|
|
|
|
int durasiGerak = selisih * 25;
|
|
|
|
if (durasiGerak < 1200) {
|
|
durasiGerak = 1200;
|
|
}
|
|
|
|
moveAll(baseMove, durasiGerak);
|
|
|
|
delay(300);
|
|
|
|
// =================================================
|
|
// TURUNKAN TELUR
|
|
// =================================================
|
|
|
|
int turun[6] = {
|
|
baseMove[0],
|
|
130,
|
|
60,
|
|
50,
|
|
35,
|
|
110
|
|
};
|
|
|
|
moveAll(turun, 1400);
|
|
|
|
delay(500);
|
|
|
|
// =================================================
|
|
// BUKA CAPIT
|
|
// =================================================
|
|
|
|
int lepas[6] = {
|
|
baseMove[0],
|
|
130,
|
|
60,
|
|
50,
|
|
55,
|
|
90
|
|
};
|
|
|
|
moveAll(lepas, 1500);
|
|
|
|
delay(500);
|
|
|
|
unsigned long tTaruh = millis();
|
|
|
|
// =================================================
|
|
// NAIK DULU
|
|
// =================================================
|
|
|
|
int amanBalik[6] = {
|
|
baseMove[0],
|
|
170,
|
|
75,
|
|
85,
|
|
55,
|
|
90
|
|
};
|
|
|
|
moveAll(amanBalik, 1400);
|
|
|
|
delay(300);
|
|
|
|
// =================================================
|
|
// BALIK BASE
|
|
// =================================================
|
|
|
|
int baseBalik[6];
|
|
|
|
for (int i = 0; i < 6; i++) {
|
|
baseBalik[i] = amanBalik[i];
|
|
}
|
|
|
|
baseBalik[0] = 137;
|
|
|
|
int selisihBalik = abs(currentPos[0] - baseBalik[0]);
|
|
|
|
int durasiBalik = selisihBalik * 25;
|
|
|
|
if (durasiBalik < 1200) {
|
|
durasiBalik = 1200;
|
|
}
|
|
|
|
moveAll(baseBalik, durasiBalik);
|
|
|
|
delay(300);
|
|
|
|
// =================================================
|
|
// POSISI STANDBY
|
|
// =================================================
|
|
|
|
int standby[6] = {
|
|
137,
|
|
160,
|
|
90,
|
|
70,
|
|
55,
|
|
90
|
|
};
|
|
|
|
moveAll(standby, 1500);
|
|
|
|
delay(300);
|
|
|
|
unsigned long tSelesai = millis();
|
|
|
|
// =================================================
|
|
// OUTPUT TIMER
|
|
// =================================================
|
|
|
|
float waktuAmbil =
|
|
(tAmbil - tStart) / 1000.0;
|
|
|
|
float waktuSortir =
|
|
(tTaruh - tAmbil) / 1000.0;
|
|
|
|
float waktuBalik =
|
|
(tSelesai - tTaruh) / 1000.0;
|
|
|
|
float totalWaktu =
|
|
(tSelesai - tStart) / 1000.0;
|
|
|
|
Serial.println("=================================");
|
|
|
|
Serial.print("Grade Tujuan : ");
|
|
Serial.println(g);
|
|
|
|
Serial.print("Waktu ambil telur : ");
|
|
Serial.print(waktuAmbil, 2);
|
|
Serial.println(" detik");
|
|
|
|
Serial.print("Waktu sortir + jatuhkan : ");
|
|
Serial.print(waktuSortir, 2);
|
|
Serial.println(" detik");
|
|
|
|
Serial.print("Waktu balik standby : ");
|
|
Serial.print(waktuBalik, 2);
|
|
Serial.println(" detik");
|
|
|
|
Serial.print("Total waktu robot : ");
|
|
Serial.print(totalWaktu, 2);
|
|
Serial.println(" detik");
|
|
|
|
Serial.println("=================================");
|
|
}
|
|
|
|
// =====================================================
|
|
// ================= WIFI ==============================
|
|
// =====================================================
|
|
|
|
void connectWiFi() {
|
|
|
|
WiFi.begin(ssid, password);
|
|
|
|
Serial.print("Connecting WiFi");
|
|
|
|
while (WiFi.status() != WL_CONNECTED) {
|
|
|
|
delay(500);
|
|
Serial.print(".");
|
|
}
|
|
|
|
Serial.println("");
|
|
Serial.println("WiFi Connected!");
|
|
}
|
|
|
|
// =====================================================
|
|
// ================= AMBIL SPEED =======================
|
|
// =====================================================
|
|
|
|
void ambilSpeed() {
|
|
|
|
if (WiFi.status() != WL_CONNECTED) return;
|
|
|
|
HTTPClient http;
|
|
|
|
http.begin(serverURL + "/getSpeed");
|
|
|
|
int code = http.GET();
|
|
|
|
if (code == 200) {
|
|
|
|
String payload = http.getString();
|
|
|
|
int idx = payload.indexOf(":");
|
|
int end = payload.indexOf("}");
|
|
|
|
if (idx != -1 && end != -1) {
|
|
|
|
int newSpeed =
|
|
payload.substring(idx + 1, end).toInt();
|
|
|
|
if (newSpeed > 0 && newSpeed != speedMotor) {
|
|
|
|
speedMotor = newSpeed;
|
|
|
|
Serial.print("Speed update: ");
|
|
Serial.println(speedMotor);
|
|
}
|
|
}
|
|
}
|
|
|
|
http.end();
|
|
}
|
|
|
|
// =====================================================
|
|
// ================= TRIGGER CAMERA ====================
|
|
// =====================================================
|
|
|
|
void triggerKamera() {
|
|
|
|
if (WiFi.status() != WL_CONNECTED) return;
|
|
|
|
HTTPClient http;
|
|
|
|
http.begin(camURL + "/capture");
|
|
|
|
http.setTimeout(8000);
|
|
|
|
int code = http.GET();
|
|
|
|
Serial.print("[CAM] Trigger status: ");
|
|
Serial.println(code);
|
|
|
|
http.end();
|
|
}
|
|
|
|
// =====================================================
|
|
// ================= JSON HELPER =======================
|
|
// =====================================================
|
|
|
|
String cariString(String body, String key) {
|
|
|
|
String cari = "\"" + key + "\"";
|
|
|
|
int idx = body.indexOf(cari);
|
|
|
|
if (idx == -1) return "";
|
|
|
|
int start =
|
|
body.indexOf("\"", idx + cari.length() + 1);
|
|
|
|
if (start == -1) return "";
|
|
|
|
int end = body.indexOf("\"", start + 1);
|
|
|
|
if (end == -1) return "";
|
|
|
|
return body.substring(start + 1, end);
|
|
}
|
|
|
|
int cariInt(String body, String key) {
|
|
|
|
String cari = "\"" + key + "\"";
|
|
|
|
int idx = body.indexOf(cari);
|
|
|
|
if (idx == -1) return -1;
|
|
|
|
int pos = idx + cari.length();
|
|
|
|
while (
|
|
pos < (int)body.length() &&
|
|
(body[pos] == ':' || body[pos] == ' ')
|
|
) pos++;
|
|
|
|
String numStr = "";
|
|
|
|
while (
|
|
pos < (int)body.length() &&
|
|
(isDigit(body[pos]) || body[pos] == '-')
|
|
) {
|
|
|
|
numStr += body[pos];
|
|
pos++;
|
|
}
|
|
|
|
if (numStr.length() == 0) return -1;
|
|
|
|
return numStr.toInt();
|
|
}
|
|
|
|
// =====================================================
|
|
// ================= POLLING ===========================
|
|
// =====================================================
|
|
|
|
bool pollingHasilKamera(
|
|
int &telurId,
|
|
String &aiGrade
|
|
) {
|
|
|
|
if (WiFi.status() != WL_CONNECTED) return false;
|
|
|
|
unsigned long start = millis();
|
|
|
|
while (millis() - start < 12000) {
|
|
|
|
HTTPClient http;
|
|
|
|
http.begin(serverURL + "/latest");
|
|
|
|
http.setTimeout(5000);
|
|
|
|
int code = http.GET();
|
|
|
|
if (code == 200) {
|
|
|
|
String body = http.getString();
|
|
|
|
int id = cariInt(body, "telur_id");
|
|
|
|
String grade = cariString(body, "grade");
|
|
|
|
if (id > 0 && grade.length() > 0) {
|
|
|
|
telurId = id;
|
|
aiGrade = grade;
|
|
|
|
http.end();
|
|
|
|
return true;
|
|
}
|
|
}
|
|
|
|
http.end();
|
|
|
|
delay(500);
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
// =====================================================
|
|
// ================= UPDATE FINAL ======================
|
|
// =====================================================
|
|
|
|
String updateFinal(
|
|
int telurId,
|
|
float berat
|
|
) {
|
|
|
|
if (WiFi.status() != WL_CONNECTED) return "TL";
|
|
|
|
HTTPClient http;
|
|
|
|
http.begin(serverURL + "/updateFinal");
|
|
|
|
http.addHeader(
|
|
"Content-Type",
|
|
"application/json"
|
|
);
|
|
|
|
String json =
|
|
"{\"id\":" + String(telurId) +
|
|
",\"berat\":" + String(berat, 2) + "}";
|
|
|
|
int code = http.POST(json);
|
|
|
|
String finalGrade = "C";
|
|
|
|
if (code == 200) {
|
|
|
|
String body = http.getString();
|
|
|
|
String fg = cariString(body, "final");
|
|
|
|
if (fg.length() > 0)
|
|
finalGrade = fg;
|
|
}
|
|
|
|
http.end();
|
|
|
|
return finalGrade;
|
|
}
|
|
|
|
// =====================================================
|
|
// ================= TIMBANG ===========================
|
|
// =====================================================
|
|
|
|
void timbangTelur() {
|
|
|
|
float smoothed = 0;
|
|
|
|
bool first = true;
|
|
|
|
unsigned long startTime = millis();
|
|
|
|
while (millis() - startTime < 3000) {
|
|
|
|
float berat = scale.get_units(10);
|
|
|
|
if (berat < 0)
|
|
berat = -berat;
|
|
|
|
if (first) {
|
|
|
|
smoothed = berat;
|
|
first = false;
|
|
}
|
|
|
|
else {
|
|
|
|
smoothed =
|
|
0.8 * berat +
|
|
0.2 * smoothed;
|
|
}
|
|
|
|
delay(60);
|
|
}
|
|
|
|
stableWeight = smoothed;
|
|
}
|
|
|
|
// =====================================================
|
|
// ================= SETUP =============================
|
|
// =====================================================
|
|
|
|
void setup() {
|
|
|
|
Serial.begin(115200);
|
|
|
|
connectWiFi();
|
|
|
|
// MOTOR
|
|
|
|
pinMode(IN1, OUTPUT);
|
|
pinMode(IN2, OUTPUT);
|
|
|
|
pinMode(IR_START, INPUT);
|
|
pinMode(IR_STOP, INPUT);
|
|
|
|
ledcSetup(PWM_CHANNEL, 1000, 8);
|
|
|
|
ledcAttachPin(ENA, PWM_CHANNEL);
|
|
|
|
digitalWrite(IN1, HIGH);
|
|
digitalWrite(IN2, LOW);
|
|
|
|
ledcWrite(PWM_CHANNEL, 0);
|
|
|
|
// LOADCELL
|
|
|
|
scale.begin(DOUT, CLK);
|
|
|
|
while (!scale.is_ready()) {
|
|
|
|
Serial.println("Menunggu HX711...");
|
|
delay(500);
|
|
}
|
|
|
|
delay(2000);
|
|
|
|
scale.set_scale(calibration_factor);
|
|
|
|
scale.tare();
|
|
|
|
// SERVO
|
|
|
|
servo1.attach(SERVO1_PIN);
|
|
servo2.attach(SERVO2_PIN);
|
|
servo3.attach(SERVO3_PIN);
|
|
servo4.attach(SERVO4_PIN);
|
|
servo5.attach(SERVO5_PIN);
|
|
servo6.attach(SERVO6_PIN);
|
|
|
|
delay(2000);
|
|
|
|
for (int i = 0; i < 6; i++) {
|
|
writeServo(i, currentPos[i]);
|
|
}
|
|
|
|
delay(1000);
|
|
|
|
posisiMati();
|
|
|
|
delay(1000);
|
|
|
|
posisiAwal();
|
|
|
|
Serial.println("=== SISTEM SIAP ===");
|
|
}
|
|
|
|
// =====================================================
|
|
// ================= LOOP ==============================
|
|
// =====================================================
|
|
|
|
void loop() {
|
|
|
|
ambilSpeed();
|
|
|
|
bool startVal = digitalRead(IR_START);
|
|
bool stopVal = digitalRead(IR_STOP);
|
|
|
|
// =================================================
|
|
// IR START
|
|
// =================================================
|
|
|
|
if (
|
|
lastStart == HIGH &&
|
|
startVal == LOW &&
|
|
!motorOn
|
|
) {
|
|
|
|
Serial.println("IR1 → Conveyor ON");
|
|
|
|
motorOn = true;
|
|
|
|
ledcWrite(PWM_CHANNEL, speedMotor);
|
|
|
|
delay(200);
|
|
}
|
|
|
|
// =================================================
|
|
// IR STOP
|
|
// =================================================
|
|
|
|
if (
|
|
lastStop == HIGH &&
|
|
stopVal == LOW &&
|
|
motorOn
|
|
) {
|
|
|
|
Serial.println("IR2 → Conveyor OFF");
|
|
|
|
motorOn = false;
|
|
|
|
ledcWrite(PWM_CHANNEL, 0);
|
|
|
|
delay(800);
|
|
|
|
// =================================================
|
|
// TIMBANG
|
|
// =================================================
|
|
|
|
timbangTelur();
|
|
|
|
Serial.print("Berat: ");
|
|
Serial.println(stableWeight);
|
|
|
|
// =================================================
|
|
// TRIGGER CAMERA
|
|
// =================================================
|
|
|
|
triggerKamera();
|
|
|
|
delay(2500);
|
|
|
|
// =================================================
|
|
// POLLING HASIL AI
|
|
// =================================================
|
|
|
|
int telurId = -1;
|
|
|
|
String aiGrade = "";
|
|
|
|
bool berhasil =
|
|
pollingHasilKamera(
|
|
telurId,
|
|
aiGrade
|
|
);
|
|
|
|
char finalChar;
|
|
|
|
// =================================================
|
|
// JIKA AI BERHASIL
|
|
// =================================================
|
|
|
|
if (berhasil) {
|
|
|
|
// =============================================
|
|
// OUTPUT HASIL AI
|
|
// =============================================
|
|
|
|
Serial.println("=================================");
|
|
|
|
Serial.print("ID Telur : ");
|
|
Serial.println(telurId);
|
|
|
|
Serial.print("Grade AI : ");
|
|
Serial.println(aiGrade);
|
|
|
|
Serial.print("Berat Telur : ");
|
|
Serial.print(stableWeight);
|
|
Serial.println(" gram");
|
|
|
|
// =============================================
|
|
// FINAL GRADE
|
|
// =============================================
|
|
|
|
String finalGrade =
|
|
updateFinal(
|
|
telurId,
|
|
stableWeight
|
|
);
|
|
|
|
Serial.print("Final Grade : ");
|
|
Serial.println(finalGrade);
|
|
|
|
Serial.println("=================================");
|
|
|
|
// =============================================
|
|
// KONVERSI FINAL GRADE
|
|
// =============================================
|
|
|
|
if (finalGrade == "A")
|
|
finalChar = 'A';
|
|
|
|
else if (finalGrade == "B")
|
|
finalChar = 'B';
|
|
|
|
else if (finalGrade == "C")
|
|
finalChar = 'C';
|
|
|
|
else
|
|
finalChar = 'T';
|
|
}
|
|
|
|
// =================================================
|
|
// JIKA AI GAGAL
|
|
// =================================================
|
|
|
|
else {
|
|
|
|
Serial.println("=================================");
|
|
Serial.println("AI Tidak Merespon");
|
|
Serial.println("Menggunakan Berat Saja");
|
|
|
|
Serial.print("Berat Telur : ");
|
|
Serial.print(stableWeight);
|
|
Serial.println(" gram");
|
|
|
|
if (stableWeight >= 70)
|
|
finalChar = 'A';
|
|
|
|
else if (stableWeight >= 60)
|
|
finalChar = 'B';
|
|
|
|
else
|
|
finalChar = 'C';
|
|
|
|
Serial.print("Final Grade : ");
|
|
Serial.println(finalChar);
|
|
|
|
Serial.println("=================================");
|
|
}
|
|
|
|
// =================================================
|
|
// ROBOT ARM
|
|
// =================================================
|
|
|
|
prosesRobot(finalChar);
|
|
|
|
delay(1000);
|
|
}
|
|
|
|
lastStart = startVal;
|
|
lastStop = stopVal;
|
|
|
|
delay(10);
|
|
}
|