/********* Pleasedontcode.com **********

	Pleasedontcode thanks you for automatic code generation! Enjoy your code!

	- Terms and Conditions:
	You have a non-exclusive, revocable, worldwide, royalty-free license
	for personal and commercial use. Attribution is optional; modifications
	are allowed, but you're responsible for code maintenance. We're not
	liable for any loss or damage. For full terms,
	please visit pleasedontcode.com/termsandconditions.

	- Project: Line Follower
	- Source Code NOT compiled for: Arduino Nano
	- Source Code created on: 2025-10-13 15:47:35

********* Pleasedontcode.com **********/

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
	/* All black stop */
/****** END SYSTEM REQUIREMENTS *****/


/* START CODE */

/****** DEFINITION OF LIBRARIES *****/

/****** FUNCTION PROTOTYPES *****/
void setup(void);
void loop(void);
int readLine();
void moveForward(int leftSpeed, int rightSpeed);
bool allSensorsBlack(void);

/****** GLOBAL VARIABLES AND PIN DEFINITIONS *****/
// Sensor pins: RLS08 sensors mapped to Nano pins (including A0A2 as analog inputs)
int sensorPins[8] = {2, 3, 4, 7, 8, A0, A1, A2}; 
int sensorValues[8];

// TB6612FNG motor driver pins mapped to Nano
#define PWMA 5   // Left motor speed (PWM)
#define PWMB 6   // Right motor speed (PWM)
#define AIN1 10
#define AIN2 11
#define BIN1 12
#define BIN2 13
#define STBY 9   // Standby

// === Motor Speed Settings ===
int baseSpeed = 150;  // normal forward speed (0255)

// === PID Variables ===
float Kp = 25;   // proportional gain
float Ki = 0.0;  // integral gain
float Kd = 15;   // derivative gain

int lastError = 0;
float integral = 0;

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

  // Sensor pins
  for (int i = 0; i < 8; i++) {
    pinMode(sensorPins[i], INPUT);
  }

  // Motor driver pins
  pinMode(PWMA, OUTPUT);
  pinMode(PWMB, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(STBY, OUTPUT);

  digitalWrite(STBY, HIGH); // Enable motor driver

  Serial.println("PID Line Follower Ready...");
}

// ----------------------------------------------------------------------
void loop(void)
{
  // All black stop condition: if all sensor inputs read LOW (0), stop the robot
  if (allSensorsBlack()) {
    moveForward(0, 0);
    Serial.println("All black stop triggered"); // new: indicate stop condition
    delay(10);
    return;
  }

  int position = readLine(); // get error from sensors
  int error = position;

  // === PID Calculation ===
  integral += error;
  int derivative = error - lastError;
  int correction = (Kp * error) + (Ki * integral) + (Kd * derivative);

  int leftSpeed = baseSpeed - correction;
  int rightSpeed = baseSpeed + correction;

  // limit speeds between 0255
  leftSpeed = constrain(leftSpeed, 0, 255);
  rightSpeed = constrain(rightSpeed, 0, 255);

  // move motors
  moveForward(leftSpeed, rightSpeed);

  lastError = error;

  delay(10); // loop delay
}

// ----------------------------------------------------------------------
// === Read Line Function ===
int readLine()
{
  int weightedSum = 0;
  int sum = 0;

  for (int i = 0; i < 8; i++) {
    sensorValues[i] = digitalRead(sensorPins[i]);

    // invert logic if needed (0=black, 1=white  so we flip)
    int value = (sensorValues[i] == 0) ? 1 : 0;

    if (value == 1) {
      weightedSum += (i * 100); // position weight (0700)
      sum += 1;
    }
  }

  if (sum == 0) {
    // no line detected  keep last error
    return lastError;
  }

  int position = weightedSum / sum;   // average position
  int error = position - 350;         // center = 350
  return error;
}

// ----------------------------------------------------------------------
// === Motor Control ===

// Move Forward
void moveForward(int leftSpeed, int rightSpeed)
{
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMA, leftSpeed);
  analogWrite(PWMB, rightSpeed);
}/* Baseline Code Empty */

// ----------------------------------------------------------------------
// Helper: check if all sensors read black (LOW)
bool allSensorsBlack(void)
{
  for (int i = 0; i < 8; i++) {
    int v = digitalRead(sensorPins[i]);
    if (v != 0) {
      return false; // at least one sensor not on black
    }
  }
  return true; // all sensors are LOW (black)
}

/* END CODE */