line follower 2

// Define motor pins
#define leftMotorForward 3
#define leftMotorBackward 4
#define leftMotorSpeed 5
#define rightMotorForward 6
#define rightMotorBackward 7
#define rightMotorSpeed 8

#define leftSensorPin 10
#define middleSensorPin 11
#define rightSensorPin 12

float kp = 1;
int setpoint = 100;
int initialSpeed = 75;

void setup() {
  pinMode(leftMotorForward, OUTPUT);
  pinMode(leftMotorBackward, OUTPUT);
  pinMode(leftMotorSpeed, OUTPUT);
  pinMode(rightMotorForward, OUTPUT);
  pinMode(rightMotorBackward, OUTPUT);
  pinMode(rightMotorSpeed, OUTPUT);

  pinMode(leftSensorPin, INPUT);
  pinMode(middleSensorPin, INPUT);
  pinMode(rightSensorPin, INPUT);
}

void loop() {
  int leftSensorValue = digitalRead(leftSensorPin);
  int middleSensorValue = digitalRead(middleSensorPin);
  int rightSensorValue = digitalRead(rightSensorPin);

  float position = (leftSensorValue * 0 + middleSensorValue * 100 + rightSensorValue * 200) / (leftSensorValue + middleSensorValue + rightSensorValue);

  float error = position - setpoint;
  float output = kp * error;

  // After drawing my chart, I found that having negative error means left is triggered so car needs to go left a bit, and positive error is opposite
  // So here motor speeds depends on if there is an error or not, if so we need to move slower until we handle the error

  // لو الخطأ كان سالب يبقى عايز اروح شمال بالشكل ده الموتور الشمال هيكون أبطأ و اليمين أسرع
  // لو الخطأ موجب يبقى عايز اروح يمين ف بالشكل ده الشمال هيكون اسرع و اليمين ابطأ
  int leftMotorSpeedValue = initialSpeed + output;
  int RightMotorSpeedValue = initialSpeed - output;

  // If I've added value to left motor so It should be moving forward, so it's greater than 0 for sure, and vice versa
  if (leftMotorSpeedValue >= 0) {
    digitalWrite(leftMotorForward, HIGH);
    digitalWrite(leftMotorBackward, LOW);
  } else {
    digitalWrite(leftMotorForward, LOW);
    digitalWrite(leftMotorBackward, HIGH);
  }

  if (RightMotorSpeedValue >= 0) {
    digitalWrite(rightMotorForward, HIGH);
    digitalWrite(rightMotorBackward, LOW);
  } else {
    digitalWrite(rightMotorForward, LOW);
    digitalWrite(rightMotorBackward, HIGH);
  }

  // Making sure to take the less value (If motor speed greater than 255 it should take 255 which is highest PWM value)
  analogWrite(leftMotorSpeed, min(abs(leftMotorSpeedValue), 255));
  analogWrite(RightMotorSpeed, min(abs(RightMotorSpeedValue), 255));
}