esp32_linefollower.ino

#include <QTRSensors.h>

// load motor library
#include "ESP32MotorControl.h"

#define KP 0.5
#define KD 0
#define M1_MAX_SPEED 70
#define M2_MAX_SPEED 70
#define M1_DEFAULT_SPEED 50
#define M2_DEFAULT_SPEED 50
#define LED 2

QTRSensors qtr;

const uint8_t SensorCount = 3;
uint16_t sensorValues[SensorCount];

// motor instance
ESP32MotorControl MotorControl = ESP32MotorControl();

void setup()
{

  Serial.begin(115200);

  // configure the sensors
  qtr.setTypeAnalog();
  qtr.setSensorPins((const uint8_t[]){36,39,34}, SensorCount);

  // Attach 2 motors
  MotorControl.attachMotors(12, 14, 27, 26);
  
  manual_calibration(); 
}

int lastError = 0;
int last_proportional = 0;
int integral = 0;


void loop()
{

  uint16_t position = qtr.readLineBlack(sensorValues);

  int error = position - 1000;

  Serial.println(position);
  Serial.println(error);

  int motorSpeed = KP * error + KD * (error - lastError);
  lastError = error;

  int rightMotorSpeed = M2_DEFAULT_SPEED - motorSpeed;
  int leftMotorSpeed = M1_DEFAULT_SPEED + motorSpeed;
  
    if (rightMotorSpeed > M1_MAX_SPEED ) rightMotorSpeed = M1_MAX_SPEED; // limit top speed
  if (leftMotorSpeed > M2_MAX_SPEED ) leftMotorSpeed = M2_MAX_SPEED; // limit top speed
  if (rightMotorSpeed < 0) rightMotorSpeed = 0; // keep motor above 0
  if (leftMotorSpeed < 0) leftMotorSpeed = 0; // keep motor speed above 0

  Serial.println(rightMotorSpeed);
  Serial.println(leftMotorSpeed);  

  MotorControl.motorForward(0, leftMotorSpeed);
  MotorControl.motorForward(1, rightMotorSpeed);

  delay(1000);
  
}

  
void manual_calibration() {

  pinMode(LED, OUTPUT);
  digitalWrite(LED, HIGH); // turn on Arduino's LED to indicate we are in calibration mode

  // analogRead() takes about 0.1 ms on an AVR.
  // 0.1 ms per sensor * 4 samples per sensor read (default) * 6 sensors
  // * 10 reads per calibrate() call = ~24 ms per calibrate() call.
  // Call calibrate() 400 times to make calibration take about 10 seconds.
  for (uint16_t i = 0; i < 8000; i++)
  {
    qtr.calibrate();
  }
  digitalWrite(LED, LOW); // turn off Arduino's LED to indicate we are through with calibration

  for (uint8_t i = 0; i < SensorCount; i++)
  {
    Serial.print(qtr.calibrationOn.minimum[i]);
    Serial.print(' ');
  }
  Serial.println();

  // print the calibration maximum values measured when emitters were on
  for (uint8_t i = 0; i < SensorCount; i++)
  {
    Serial.print(qtr.calibrationOn.maximum[i]);
    Serial.print(' ');
  }
  Serial.println();
  Serial.println();
  delay(1000);

}