Untitled

#include <QTRSensors.h>

// Configuratie senzori QTR-8
#define NUM_SENSORS 8
#define EMITTER_PIN 2 // Define your emitter pin
QTRSensorsAnalog qtr((unsigned char[]){32, 33, 34, 35, 36, 39, 25, 26}, NUM_SENSORS, EMITTER_PIN);

// Pini control motor cu driver DRV
#define MOTOR_LEFT_PWM 32
#define MOTOR_LEFT_DIR 33
#define MOTOR_RIGHT_PWM 22
#define MOTOR_RIGHT_DIR 23

// Constante PID pentru linie
float Kp_line = 1.2;  
float Ki_line = 0.0;  
float Kd_line = 0.8;  

int baseSpeed = 200; // Viteza de baza a motoarelor
int maxSpeed = 255;  // Viteza maxima a motoarelor

// Variabile pentru functionarea PID (urmarire linie)
float error_line = 0, lastError_line = 0, integral_line = 0, derivative_line = 0;
unsigned int position = 0;

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

  // Initializare senzori QTR-8
  qtr.setTypeAnalog();
  qtr.setSensorPins((unsigned char[]){32, 33, 34, 35, 36, 39, 25, 26}, NUM_SENSORS);
  qtr.setEmitterPin(EMITTER_PIN);

  // Stare pini motor
  pinMode(MOTOR_LEFT_PWM, OUTPUT);
  pinMode(MOTOR_LEFT_DIR, OUTPUT);
  pinMode(MOTOR_RIGHT_PWM, OUTPUT);
  pinMode(MOTOR_RIGHT_DIR, OUTPUT);

  // Calibrare senzori
  calibrateSensors();
}

void loop() {
  followLine();
}

/////////////////FUNCTIA DE URMARIRE A LINIEI//////////////////////

void followLine() {
  unsigned int sensorValues[NUM_SENSORS];
  position = qtr.readLine(sensorValues);

  error_line = position - 3500; 
  integral_line += error_line;
  derivative_line = error_line - lastError_line;

  int motorSpeedAdjustment = Kp_line * error_line + Ki_line * integral_line + Kd_line * derivative_line;

  int leftMotorSpeed = baseSpeed - motorSpeedAdjustment;
  int rightMotorSpeed = baseSpeed + motorSpeedAdjustment;

  setMotorSpeed(leftMotorSpeed, rightMotorSpeed);
  lastError_line = error_line;
}

/////////////////FUNCTIA DE SETARE A VITEZEI MOTORULUI//////////////////////

void setMotorSpeed(int leftSpeed, int rightSpeed) {
  // Constrangem vitezele in limitele specificate
  leftSpeed = constrain(leftSpeed, -maxSpeed, maxSpeed);
  rightSpeed = constrain(rightSpeed, -maxSpeed, maxSpeed);

  // Setare motor stanga
  if (leftSpeed >= 0) {
    digitalWrite(MOTOR_LEFT_DIR, HIGH);
    analogWrite(MOTOR_LEFT_PWM, leftSpeed);
  } else {
    digitalWrite(MOTOR_LEFT_DIR, LOW);
    analogWrite(MOTOR_LEFT_PWM, -leftSpeed);
  }

  // Setare motor dreapta
  if (rightSpeed >= 0) {
    digitalWrite(MOTOR_RIGHT_DIR, HIGH);
    analogWrite(MOTOR_RIGHT_PWM, rightSpeed);
  } else {
    digitalWrite(MOTOR_RIGHT_DIR, LOW);
    analogWrite(MOTOR_RIGHT_PWM, -rightSpeed);
  }
}

/////////////////RUTINA DE CALIBRARE A SENZORILOR//////////////////////

void calibrateSensors() {
  Serial.println("Calibrating sensors...");
  for (int i = 0; i < 400; i++) { // Calibrare pentru 4 secunde
    qtr.calibrate();
    delay(10);
  }
  Serial.println("Calibration done.");
}