Arduino_Code

// ===== 2026 HACKATHON ROBOT: OFF-ROAD FINAL VERSION =====
// Key Features:
// 1. High torque movement tuning for uneven terrain (anti-stuck design)
// 2. Black-line starting zone detection logic
// 3. Obstacle avoidance using 90° exit + segmented aggressive return
//    (Large ~20cm turning radius for stable bypass)

#define IN1 4
#define IN2 5
#define IN3 6
#define IN4 7

// TCS3200 Color Sensor Pins
#define S0 2
#define S1 3
#define S2 A0
#define S3 A1
#define COLOR_OUT A2

// Ultrasonic Sensor Pins
#define TRIG 12
#define ECHO 11

int mode = 0; 
int stage = 0; // 0 = Start Zone Phase, 1 = Competition Phase

void setup() {
  Serial.begin(9600);

  // Motor driver pins
  pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);

  // Color sensor control pins
  pinMode(S0, OUTPUT); pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT); pinMode(S3, OUTPUT);
  pinMode(COLOR_OUT, INPUT);

  // Ultrasonic sensor pins
  pinMode(TRIG, OUTPUT); 
  pinMode(ECHO, INPUT);

  // TCS3200 output frequency scaling set to 20%
  digitalWrite(S0, HIGH);
  digitalWrite(S1, LOW);

  stopMotor();
  Serial.println("===== OFF-ROAD MODE READY =====");
}

void loop() {

  // ===== Manual Control via Serial (Debug Mode) =====
  if (Serial.available()) {
    char cmd = Serial.read();

    if (cmd == '0') { 
      mode = 0;
      stage = 0;
      stopMotor();
      Serial.println("SYSTEM RESET -> STAGE 0");
    }
    else if (cmd == '1') mode = 1;
    else if (cmd == 'w') forward(); 
    else if (cmd == 's') backward();
    else if (cmd == 'a') hardLeft();    
    else if (cmd == 'd') hardRight();   
    else if (cmd == 'x') stopMotor();
  }

  // ===== Autonomous Mode =====
  if (mode == 1) {
    competitionRun();
  }
}

// =====================================================
// ================= MAIN COMPETITION LOGIC ============
// =====================================================
void competitionRun() {

  float dist = getDist();
  String color = getColor(); 

  // ---------- STAGE 0 : START ZONE (Black Track Entry) ----------
  if (stage == 0) {

    // Adjust direction while following black starting line
    if (color == "BLACK") {
      softLeft(); 
      delay(35);
    }
    else if (color == "WHITE") {
      softRight(); 
      delay(40);
    }

    // Detect red marker → enter competition phase
    else if (color == "RED") {

      Serial.println("ENTERING STAGE 1");

      // Strong forward push to avoid being stuck at junction
      forward(); 
      delay(250); 

      // Left turn into main track
      hardLeft(); 
      delay(2800); 

      stage = 1;
    }

    return;
  }

  // ---------- STAGE 1 : MAIN RACE PHASE ----------

  // 1. Obstacle detected OR black boundary detected → Avoid
  if (color == "BLACK" || (dist < 15 && dist > 1)) {
    avoidObstacle_OffRoad();
    return;
  }

  // 2. Blue zone: Pickup simulation
  if (color == "BLUE") {
    stopMotor(); 
    delay(3000); 
    forward(); 
    delay(500); 
    return;
  }

  // 3. Green zone: Slow movement (speed control)
  if (color == "GREEN") {
    softLeft(); 
    delay(30);
    stopMotor(); 
    delay(20);
  }

  // 4. Red line tracking
  else if (color == "RED") {
    softLeft(); 
    delay(35);
  }

  // 5. White background → searching for track
  else {
    softRight(); 
    delay(40);
  }
}

// =====================================================
// ================= OFF-ROAD OBSTACLE AVOIDANCE =======
// =====================================================
void avoidObstacle_OffRoad() {

  Serial.println(">>> OFF-ROAD OBSTACLE AVOIDANCE");

  // Step 1: Strong reverse to escape terrain pits
  stopMotor(); 
  delay(100);

  backward();  
  delay(450);

  // Step 2: Pivot left approx. 90 degrees
  Serial.println(">>> ROTATING LEFT");
  hardLeft(); 
  delay(2800);

  // Step 3: Drive forward to clear obstacle width
  Serial.println(">>> DRIVING OUT");
  forward();
  delay(1200);

  // Step 4: Segmented aggressive turning return
  Serial.println(">>> SEGMENTED RETURN");

  unsigned long blindStart = millis();

  while (millis() - blindStart < 2500) {

    // Hard steering correction
    hardRight(); 
    delay(200);

    // High momentum forward burst
    forward();   
    delay(350);
  }

  // Step 5: Search red line and lock-on
  Serial.println(">>> SEARCHING TRACK");

  unsigned long searchStart = millis();

  while (true) {

    hardRight(); 
    delay(200);

    forward();   
    delay(300);

    // Stop immediately when red line found
    if (getColor() == "RED") {
      stopMotor(); 
      delay(100); 
      break;
    }

    // Safety timeout protection
    if (millis() - searchStart > 6000) {
      stopMotor();