Versi Yang Anda Susun Sudah Sangat Lengkap Namun

#include <Arduino.h>
#include <Wire.h>
#include <Servo.h>
#include <LiquidCrystal_I2C.h>
#include <TinyGPS++.h>
#include <Adafruit_MLX90614.h>
#include <Adafruit_INA219.h>
#include <SD.h>

#define LEFT_LPWM 6
#define RIGHT_RPWM 7
#define RIGHT_LPWM 8
#define ECHO_PIN 27
#define SERVO_PIN 9
#define MODE_PIN 44
#define FC_RIGHT 29
#define LINE_LEFT 30
#define LINE_RIGHT 31
#define MQ135_PIN 55
#define SD_CS 53
#define BUZZER 45


// Forward declarations
void drive(int l,int r);
void stopDrive();
long distanceCm();
void manualControl();
void autonomous();

const uint8_t LEFT_RPWM=5, LEFT_LPWM=6, RIGHT_RPWM=7, RIGHT_LPWM=8;
const uint8_t LEFT_REN=22, LEFT_LEN=23, RIGHT_REN=24, RIGHT_LEN=25;
const uint8_t TRIG_PIN=26, ECHO_PIN=27, SERVO_PIN=9, MODE_PIN=44;
const uint8_t FC_LEFT=28, FC_RIGHT=29, LINE_LEFT=30, LINE_RIGHT=31;
const uint8_t MQ2_PIN=A0, MQ135_PIN=A1, SD_CS=53;
const uint8_t LED_GREEN=10, LED_RED=11, LED_BLUE=12, LED_YELLOW=13, LED_WHITE=46, BUZZER=45;
LiquidCrystal_I2C lcdNav(0x27,16,2), lcdEnv(0x26,16,2);
TinyGPSPlus gps; Adafruit_MLX90614 mlx; Adafruit_INA219 ina219; Servo scanServo;
bool sdOK=false; unsigned long lastDisplay=0,lastLog=0;
void drive(int l,int r){digitalWrite(LEFT_REN,HIGH);digitalWrite(LEFT_LEN,HIGH);digitalWrite(RIGHT_REN,HIGH);digitalWrite(RIGHT_LEN,HIGH);analogWrite(LEFT_RPWM,l>0?l:0);analogWrite(LEFT_LPWM,l<0?-l:0);analogWrite(RIGHT_RPWM,r>0?r:0);analogWrite(RIGHT_LPWM,r<0?-r:0);}
void stopDrive(){drive(0,0);}
long distanceCm(){digitalWrite(TRIG_PIN,LOW);delayMicroseconds(2);digitalWrite(TRIG_PIN,HIGH);delayMicroseconds(10);digitalWrite(TRIG_PIN,LOW);unsigned long t=pulseIn(ECHO_PIN,HIGH,25000);return t?t/58:400;}
void manualControl(){while(Serial2.available()){char c=Serial2.read(); if(c=='F')drive(170,170); else if(c=='B')drive(-170,-170); else if(c=='L')drive(-150,150); else if(c=='R')drive(150,-150); else if(c=='S')stopDrive();}}
void autonomous(){long d=distanceCm(); bool blocked=(d<25)||(digitalRead(FC_LEFT)==LOW)||(digitalRead(FC_RIGHT)==LOW); if(blocked){stopDrive();digitalWrite(LED_RED,HIGH);digitalWrite(BUZZER,HIGH);delay(180);drive(-140,-140);delay(280);drive(150,-150);delay(350);stopDrive();}else{digitalWrite(LED_RED,LOW);digitalWrite(BUZZER,LOW); bool l=digitalRead(LINE_LEFT)==LOW,r=digitalRead(LINE_RIGHT)==LOW; if(l&&!r)drive(100,180); else if(r&&!l)drive(180,100); else drive(155,155);}}
void setup(){pinMode(TRIG_PIN,OUTPUT);pinMode(ECHO_PIN,INPUT);pinMode(MODE_PIN,INPUT_PULLUP);pinMode(FC_LEFT,INPUT);pinMode(FC_RIGHT,INPUT);pinMode(LINE_LEFT,INPUT);pinMode(LINE_RIGHT,INPUT);pinMode(BUZZER,OUTPUT);for(uint8_t p: {LED_GREEN,LED_RED,LED_BLUE,LED_YELLOW,LED_WHITE})pinMode(p,OUTPUT);Serial.begin(115200);Serial1.begin(9600);Serial2.begin(9600);Wire.begin();lcdNav.init();lcdNav.backlight();lcdEnv.init();lcdEnv.backlight();scanServo.attach(SERVO_PIN);scanServo.write(90);mlx.begin();ina219.begin();sdOK=SD.begin(SD_CS);digitalWrite(LED_GREEN,HIGH);}
void loop(){while(Serial1.available())gps.encode(Serial1.read());bool autonomousMode=digitalRead(MODE_PIN)==LOW;if(autonomousMode)autonomous();else manualControl();if(millis()-lastDisplay>500){lastDisplay=millis();float obj=mlx.readObjectTempC();float v=ina219.getBusVoltage_V();int mq2=analogRead(MQ2_PIN),mq135=analogRead(MQ135_PIN);lcdNav.setCursor(0,0);lcdNav.print(autonomousMode?"AUTO ":"MANUAL");lcdNav.print(" D:");lcdNav.print(distanceCm());lcdNav.print(" ");lcdNav.setCursor(0,1);if(gps.location.isValid()){lcdNav.print(gps.location.lat(),4);lcdNav.print(",");lcdNav.print(gps.location.lng(),4);}else lcdNav.print("GPS searching...");lcdEnv.setCursor(0,0);lcdEnv.print("T:");lcdEnv.print(obj,1);lcdEnv.print("C V:");lcdEnv.print(v,1);lcdEnv.setCursor(0,1);lcdEnv.print("M2:");lcdEnv.print(mq2);lcdEnv.print(" M135:");lcdEnv.print(mq135);digitalWrite(LED_BLUE,gps.location.isValid());digitalWrite(LED_YELLOW,sdOK);digitalWrite(LED_WHITE,obj>45);if(sdOK&&millis()-lastLog>5000){lastLog=millis();File f=SD.open("arv.csv",FILE_WRITE);if(f){f.print(millis());f.print(',');f.print(autonomousMode);f.print(',');f.print(distanceCm());f.print(',');f.print(obj);f.print(',');f.print(mq2);f.print(',');f.println(mq135);f.close();}}}}