/* Wokwi Simulator - Robot Line Follower 6 Sensor + PID + OLED + 5-Way Navigation Board: ESP32 DevKit V1 Cara simulasi di Wokwi: 1. Buat project baru ESP32. 2. Upload file sketch.ino ini. 3. Upload diagram.json yang disediakan. 4. Jalankan simulasi. 5. Gunakan 6 switch sensor: - ON = sensor membaca garis hitam - OFF = sensor membaca putih 6. Gunakan tombol UP/DOWN/LEFT/RIGHT/OK untuk tuning PID di OLED. Catatan: - Ini adalah simulator logika dan kontrol PID, bukan simulasi fisik lintasan sebenarnya. - Motor kiri/kanan divisualkan dengan LED PWM dan/atau DC motor Wokwi. - Di robot asli, input switch diganti dengan 6 sensor IR. */ #include #include #include #include #include // ================= OLED ================= #define SCREEN_WIDTH 128 #define SCREEN_HEIGHT 64 #define OLED_RESET -1 #define OLED_ADDR 0x3C Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET); Preferences prefs; // ================= Sensor ================= // Pada Wokwi, slide switch ON menghubungkan GPIO ke GND. // Jadi sensor aktif = LOW. const bool SENSOR_ACTIVE_LOW = true; const uint8_t NUM_SENSORS = 6; // Urutan sensor kiri ke kanan: S1 S2 S3 S4 S5 S6 const uint8_t sensorPin[NUM_SENSORS] = {34, 35, 32, 33, 25, 26}; const int sensorWeight[NUM_SENSORS] = {-5, -3, -1, 1, 3, 5}; // ================= 5-Way Navigation Switch ================= // Pushbutton ditekan = LOW karena memakai internal pull-up. const uint8_t BTN_UP = 13; const uint8_t BTN_DOWN = 14; const uint8_t BTN_LEFT = 27; const uint8_t BTN_RIGHT = 12; const uint8_t BTN_CENTER = 23; const unsigned long DEBOUNCE_MS = 160; // ================= Motor Driver L298N Simulation ================= // Dalam Wokwi, output PWM juga dihubungkan ke LED kiri/kanan. // Jika memakai DC motor virtual, ENA/ENB menjadi PWM motor. const uint8_t ENA = 5; const uint8_t IN1 = 18; const uint8_t IN2 = 19; const uint8_t ENB = 17; const uint8_t IN3 = 16; const uint8_t IN4 = 4; const uint8_t PWM_CH_LEFT = 0; const uint8_t PWM_CH_RIGHT = 1; const uint16_t PWM_FREQ = 15000; const uint8_t PWM_RES = 8; // ================= PID ================= float Kp = 24.0; float Ki = 0.00; float Kd = 12.0; int baseSpeed = 120; int maxSpeed = 210; float integral = 0; float lastError = 0; unsigned long lastPIDTime = 0; const float INTEGRAL_LIMIT = 100.0; // ================= State Machine ================= enum RobotState { STATE_IDLE, STATE_FOLLOW_LINE, STATE_JUNCTION, STATE_TURNING, STATE_STOP }; enum TrackType { TRACK_UNKNOWN, TRACK_STRAIGHT, TRACK_LEFT_TURN, TRACK_RIGHT_TURN, TRACK_LEFT_T, TRACK_RIGHT_T, TRACK_T, TRACK_CROSS, TRACK_UTURN, TRACK_END }; enum MenuItem { MENU_KP, MENU_KI, MENU_KD, MENU_BASE, MENU_MAX, MENU_RUN }; RobotState state = STATE_IDLE; TrackType currentTrack = TRACK_UNKNOWN; MenuItem menu = MENU_RUN; bool running = false; unsigned long lastDisplay = 0; unsigned long lastButton = 0; unsigned long lineLostSince = 0; const unsigned long LINE_LOST_UTURN_MS = 180; const int JUNCTION_ACTIVE_COUNT = 4; // ================= Nama Status ================= const char* trackName(TrackType t) { switch (t) { case TRACK_STRAIGHT: return "STRAIGHT"; case TRACK_LEFT_TURN: return "LEFT"; case TRACK_RIGHT_TURN: return "RIGHT"; case TRACK_LEFT_T: return "LEFT_T"; case TRACK_RIGHT_T: return "RIGHT_T"; case TRACK_T: return "T"; case TRACK_CROSS: return "CROSS"; case TRACK_UTURN: return "U-TURN"; case TRACK_END: return "END"; default: return "UNKNOWN"; } } const char* stateName(RobotState s) { switch (s) { case STATE_IDLE: return "IDLE"; case STATE_FOLLOW_LINE: return "FOLLOW"; case STATE_JUNCTION: return "JUNCTION"; case STATE_TURNING: return "TURNING"; case STATE_STOP: return "STOP"; default: return "?"; } } // ================= Motor ================= void setMotorRaw(int leftPWM, int rightPWM) { leftPWM = constrain(leftPWM, -255, 255); rightPWM = constrain(rightPWM, -255, 255); digitalWrite(IN1, leftPWM >= 0 ? HIGH : LOW); digitalWrite(IN2, leftPWM >= 0 ? LOW : HIGH); digitalWrite(IN3, rightPWM >= 0 ? HIGH : LOW); digitalWrite(IN4, rightPWM >= 0 ? LOW : HIGH); ledcWrite(ENA, abs(leftPWM)); ledcWrite(ENB, abs(rightPWM)); } void stopMotor() { ledcWrite(ENA, 0); ledcWrite(ENB, 0); digitalWrite(IN1, LOW); digitalWrite(IN2, LOW); digitalWrite(IN3, LOW); digitalWrite(IN4, LOW); } // ================= Sensor ================= uint8_t readSensors(bool s[NUM_SENSORS]) { uint8_t activeCount = 0; for (uint8_t i = 0; i < NUM_SENSORS; i++) { int raw = digitalRead(sensorPin[i]); bool active = SENSOR_ACTIVE_LOW ? (raw == LOW) : (raw == HIGH); s[i] = active; if (active) activeCount++; } return activeCount; } String sensorPattern(const bool s[NUM_SENSORS]) { String p = ""; for (uint8_t i = 0; i < NUM_SENSORS; i++) { p += s[i] ? "1" : "0"; } return p; } float computeLineError(const bool s[NUM_SENSORS]) { long weightedSum = 0; int activeCount = 0; for (uint8_t i = 0; i < NUM_SENSORS; i++) { if (s[i]) { weightedSum += sensorWeight[i]; activeCount++; } } if (activeCount == 0) { return lastError >= 0 ? 6.0 : -6.0; } return (float)weightedSum / (float)activeCount; } // ================= Deteksi Lintasan ================= TrackType detectTrack(const bool s[NUM_SENSORS], uint8_t activeCount) { bool leftStrong = s[0] || s[1]; bool midStrong = s[2] || s[3]; bool rightStrong = s[4] || s[5]; if (activeCount == 0) return TRACK_UTURN; if (midStrong && !leftStrong && !rightStrong) return TRACK_STRAIGHT; if (leftStrong && !rightStrong && !midStrong) return TRACK_LEFT_TURN; if (rightStrong && !leftStrong && !midStrong) return TRACK_RIGHT_TURN; if (leftStrong && midStrong && !rightStrong) return TRACK_LEFT_T; if (rightStrong && midStrong && !leftStrong) return TRACK_RIGHT_T; if (activeCount >= 5) return TRACK_CROSS; return TRACK_UNKNOWN; } // ================= PID Line Following ================= void followLinePID() { bool s[NUM_SENSORS]; uint8_t activeCount = readSensors(s); if (activeCount == 0) { if (lineLostSince == 0) lineLostSince = millis(); if (millis() - lineLostSince > LINE_LOST_UTURN_MS) { currentTrack = TRACK_UTURN; state = STATE_TURNING; return; } } else { lineLostSince = 0; } currentTrack = detectTrack(s, activeCount); if (activeCount >= JUNCTION_ACTIVE_COUNT && currentTrack != TRACK_STRAIGHT) { state = STATE_JUNCTION; return; } float error = computeLineError(s); unsigned long now = millis(); float dt = (now - lastPIDTime) / 1000.0; if (dt <= 0 || dt > 0.1) dt = 0.01; lastPIDTime = now; integral += error * dt; integral = constrain(integral, -INTEGRAL_LIMIT, INTEGRAL_LIMIT); float derivative = (error - lastError) / dt; float correction = Kp * error + Ki * integral + Kd * derivative; lastError = error; int leftSpeed = constrain((int)(baseSpeed - correction), -maxSpeed, maxSpeed); int rightSpeed = constrain((int)(baseSpeed + correction), -maxSpeed, maxSpeed); setMotorRaw(leftSpeed, rightSpeed); } // ================= Gerakan Persimpangan ================= void moveForwardShort(uint16_t ms = 120, int spd = 95) { setMotorRaw(spd, spd); delay(ms); stopMotor(); delay(30); } void turnLeft90() { setMotorRaw(-105, 105); delay(330); stopMotor(); } void turnRight90() { setMotorRaw(105, -105); delay(330); stopMotor(); } void turnAround180() { setMotorRaw(110, -110); delay(650); stopMotor(); } void handleJunction() { moveForwardShort(120, 90); bool s[NUM_SENSORS]; uint8_t activeCount = readSensors(s); // Pada simulasi, jika semua switch sensor ON terus, dianggap END. if (activeCount >= 5) { delay(120); activeCount = readSensors(s); if (activeCount >= 5) { currentTrack = TRACK_END; state = STATE_STOP; return; } } switch (currentTrack) { case TRACK_LEFT_TURN: turnLeft90(); break; case TRACK_RIGHT_TURN: turnRight90(); break; case TRACK_T: turnLeft90(); break; case TRACK_LEFT_T: case TRACK_RIGHT_T: case TRACK_CROSS: default: // Strategi default: lurus break; } integral = 0; lastError = 0; state = STATE_FOLLOW_LINE; } // ================= Menu Tombol ================= bool pressed(uint8_t pin) { if (digitalRead(pin) == LOW && millis() - lastButton > DEBOUNCE_MS) { lastButton = millis(); return true; } return false; } void saveParams() { prefs.begin("pid", false); prefs.putFloat("kp", Kp); prefs.putFloat("ki", Ki); prefs.putFloat("kd", Kd); prefs.putInt("base", baseSpeed); prefs.putInt("max", maxSpeed); prefs.end(); } void loadParams() { prefs.begin("pid", true); Kp = prefs.getFloat("kp", Kp); Ki = prefs.getFloat("ki", Ki); Kd = prefs.getFloat("kd", Kd); baseSpeed = prefs.getInt("base", baseSpeed); maxSpeed = prefs.getInt("max", maxSpeed); prefs.end(); } void handleButtons() { if (pressed(BTN_LEFT)) { menu = (MenuItem)((menu + 5) % 6); } if (pressed(BTN_RIGHT)) { menu = (MenuItem)((menu + 1) % 6); } if (pressed(BTN_UP)) { switch (menu) { case MENU_KP: Kp += 0.5; break; case MENU_KI: Ki += 0.01; break; case MENU_KD: Kd += 0.5; break; case MENU_BASE: baseSpeed += 5; break; case MENU_MAX: maxSpeed += 5; break; case MENU_RUN: running = true; state = STATE_FOLLOW_LINE; break; } } if (pressed(BTN_DOWN)) { switch (menu) { case MENU_KP: Kp -= 0.5; break; case MENU_KI: Ki -= 0.01; break; case MENU_KD: Kd -= 0.5; break; case MENU_BASE: baseSpeed -= 5; break; case MENU_MAX: maxSpeed -= 5; break; case MENU_RUN: running = false; state = STATE_IDLE; stopMotor(); break; } } Kp = max(0.0f, Kp); Ki = max(0.0f, Ki); Kd = max(0.0f, Kd); baseSpeed = constrain(baseSpeed, 0, 255); maxSpeed = constrain(maxSpeed, 0, 255); if (pressed(BTN_CENTER)) { saveParams(); if (menu == MENU_RUN) { running = !running; state = running ? STATE_FOLLOW_LINE : STATE_IDLE; if (!running) stopMotor(); } } } // ================= OLED ================= void drawOLED() { if (millis() - lastDisplay < 120) return; lastDisplay = millis(); bool s[NUM_SENSORS]; readSensors(s); display.clearDisplay(); display.setTextColor(SSD1306_WHITE); display.setTextSize(1); display.setCursor(0, 0); display.print("STATE:"); display.print(stateName(state)); display.setCursor(0, 10); display.print("TRACK:"); display.print(trackName(currentTrack)); display.setCursor(0, 20); display.print("S:"); display.print(sensorPattern(s)); display.setCursor(0, 32); switch (menu) { case MENU_KP: display.print(">KP "); display.print(Kp, 2); break; case MENU_KI: display.print(">KI "); display.print(Ki, 2); break; case MENU_KD: display.print(">KD "); display.print(Kd, 2); break; case MENU_BASE: display.print(">BASE "); display.print(baseSpeed); break; case MENU_MAX: display.print(">MAX "); display.print(maxSpeed); break; case MENU_RUN: display.print(">RUN "); display.print(running ? "ON" : "OFF"); break; } display.setCursor(0, 44); display.print("L/R Menu U/D Val"); display.setCursor(0, 56); display.print("OK Save/Run"); display.display(); } // ================= Setup ================= void setup() { Serial.begin(115200); // Sensor switch Wokwi memakai pull-up internal. for (uint8_t i = 0; i < NUM_SENSORS; i++) { pinMode(sensorPin[i], INPUT_PULLUP); } pinMode(BTN_UP, INPUT_PULLUP); pinMode(BTN_DOWN, INPUT_PULLUP); pinMode(BTN_LEFT, INPUT_PULLUP); pinMode(BTN_RIGHT, INPUT_PULLUP); pinMode(BTN_CENTER, INPUT_PULLUP); pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT); pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT); // Arduino-ESP32 core 3.x / Wokwi compatible PWM setup ledcAttach(ENA, PWM_FREQ, PWM_RES); ledcAttach(ENB, PWM_FREQ, PWM_RES); stopMotor(); Wire.begin(21, 22); // SDA = GPIO21, SCL = GPIO22 if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) { Serial.println("OLED tidak terdeteksi"); } loadParams(); display.clearDisplay(); display.setTextColor(SSD1306_WHITE); display.setTextSize(1); display.setCursor(0, 0); display.println("LINE FOLLOWER"); display.println("WOKWI SIM"); display.println("6 SENSOR + PID"); display.println("OLED + 5-WAY"); display.println(""); display.println("OK / UP = RUN"); display.display(); delay(1500); lastPIDTime = millis(); } // ================= Loop ================= void loop() { handleButtons(); switch (state) { case STATE_IDLE: stopMotor(); currentTrack = TRACK_UNKNOWN; break; case STATE_FOLLOW_LINE: followLinePID(); break; case STATE_JUNCTION: handleJunction(); break; case STATE_TURNING: if (currentTrack == TRACK_UTURN) { turnAround180(); state = STATE_FOLLOW_LINE; } break; case STATE_STOP: stopMotor(); running = false; break; } drawOLED(); }