Робот едет по черной линии

Доброго времени суток ВСЕМ! Подскажите пожалуйста, как добавить третий датчик линии роботу? Робот - старый робот пылесос. Питание от аккумулятора, не родного но подходящего по характеристикам. Управление Arduino uno (амперка),Motorshield (амперка). Питание arduino от 12 вольт аккумулятора. Питание раздельное. С двумя датчиками линии по данному примеру работает отлично

Код (C++):

// Моторы подключаются к клеммам M1+,M1-,M2+,M2-
// Motor shield использует четыре контакта 6,5,7,4 для управления моторами
#define SPEED_LEFT       6
#define SPEED_RIGHT      5
#define DIR_LEFT         7
#define DIR_RIGHT        4
#define LEFT_SENSOR_PIN  A3
#define RIGHT_SENSOR_PIN A5
// Скорость, с которой мы движемся вперёд (0-255)
#define SPEED            155
// Скорость прохождения сложных участков
#define SLOW_SPEED       25
#define BACK_SLOW_SPEED  15
#define BACK_FAST_SPEED  40
// Коэффициент, задающий во сколько раз нужно затормозить
// одно из колёс для поворота
#define BRAKE_K          5
#define STATE_FORWARD    0
#define STATE_RIGHT      1
#define STATE_LEFT       2
#define SPEED_STEP       2
#define FAST_TIME_THRESHOLD     300
int state = STATE_FORWARD;
int currentSpeed = SPEED;
int fastTime = 0;
void runForward()
{
    state = STATE_FORWARD;
    fastTime += 1;
    if (fastTime < FAST_TIME_THRESHOLD) {
        currentSpeed = SLOW_SPEED;
    } else {
        currentSpeed = min(currentSpeed + SPEED_STEP, SPEED);
    }
    analogWrite(SPEED_LEFT, currentSpeed);
    analogWrite(SPEED_RIGHT, currentSpeed);
    digitalWrite(DIR_LEFT, HIGH);
    digitalWrite(DIR_RIGHT, HIGH);
}
void steerRight()
{
    state = STATE_RIGHT;
    fastTime = 0;
    // Замедляем правое колесо относительно левого,
    // чтобы начать поворот
    analogWrite(SPEED_RIGHT, 0);
    analogWrite(SPEED_LEFT, SPEED);
    digitalWrite(DIR_LEFT, HIGH);
    digitalWrite(DIR_RIGHT, HIGH);
}
void steerLeft()
{
    state = STATE_LEFT;
    fastTime = 0;
    analogWrite(SPEED_LEFT, 0);
    analogWrite(SPEED_RIGHT, SPEED);
    digitalWrite(DIR_LEFT, HIGH);
    digitalWrite(DIR_RIGHT, HIGH);
}
void stepBack(int duration, int state) {
    if (!duration)
        return;
    // В зависимости от направления поворота при движении назад будем
    // делать небольшой разворот
    int leftSpeed = (state == STATE_RIGHT) ? BACK_SLOW_SPEED : BACK_FAST_SPEED;
    int rightSpeed = (state == STATE_LEFT) ? BACK_SLOW_SPEED : BACK_FAST_SPEED;
    analogWrite(SPEED_LEFT, leftSpeed);
    analogWrite(SPEED_RIGHT, rightSpeed);
    // реверс колёс
    digitalWrite(DIR_RIGHT, LOW);
    digitalWrite(DIR_LEFT, LOW);
    delay(duration);
}
void setup()
{
    // Настраивает выводы платы 4,5,6,7 на вывод сигналов
    for(int i = 4; i <= 7; i++)
        pinMode(i, OUTPUT);
    // Сразу едем вперёд
    runForward();
}
void loop()
{
    // Наш робот ездит по белому полю с чёрным треком. В обратном случае не нужно
    // инвертировать значения с датчиков
    boolean left = !digitalRead(LEFT_SENSOR_PIN);
    boolean right = !digitalRead(RIGHT_SENSOR_PIN);
    // В какое состояние нужно перейти?
    int targetState;
    if (left == right) {
        // под сенсорами всё белое или всё чёрное
        // едем вперёд
        targetState = STATE_FORWARD;
    } else if (left) {
        // левый сенсор упёрся в трек
        // поворачиваем налево
        targetState = STATE_LEFT;
    } else {
        targetState = STATE_RIGHT;
    }
    if (state == STATE_FORWARD && targetState != STATE_FORWARD) {
        int brakeTime = (currentSpeed > SLOW_SPEED) ?
            currentSpeed : 0;
        stepBack(brakeTime, targetState);
    }
    switch (targetState) {
        case STATE_FORWARD:
            runForward();
            break;
        case STATE_RIGHT:
            steerRight();
            break;
        case STATE_LEFT:
            steerLeft();
            break;
    }
}

Нужно добавить третий датчик линии и функцию остановки когда три датчика на черной линии. Так же при включении питания-задний ход, разворот и выезд на черную линию. Пробовал несколько похожих примеров, но нет движения вообще:

Код (C++):

// Include necessary libraries
#include <AFMotor.h>

// Define motor shield pins
#define MOTOR1 1
#define MOTOR2 2

// Define line sensors pins
#define LINE_SENSOR1 A3
#define LINE_SENSOR2 A4
#define LINE_SENSOR3 A5

// Create motor shield object
AF_DCMotor motor1(MOTOR1);
AF_DCMotor motor2(MOTOR2);

// Define variables for line sensor readings
int sensor1, sensor2, sensor3;

void setup() {
  // Initialize serial communication
  Serial.begin(9600);

  // Set motor speed
  motor1.setSpeed(255);
  motor2.setSpeed(255);

  // Set line sensor pins as inputs
  pinMode(LINE_SENSOR1, INPUT);
  pinMode(LINE_SENSOR2, INPUT);
  pinMode(LINE_SENSOR3, INPUT);
}

void loop() {
  // Read line sensor values
  sensor1 = digitalRead(LINE_SENSOR1);
  sensor2 = digitalRead(LINE_SENSOR2);
  sensor3 = digitalRead(LINE_SENSOR3);

  // If all three sensors are on black line, stop
  if (sensor1 == LOW && sensor2 == LOW && sensor3 == LOW) {
    motor1.run(RELEASE);
    motor2.run(RELEASE);
  }
  // If only sensor1 is on black line, turn left
  else if (sensor1 == LOW) {
    motor1.run(FORWARD);
    motor2.run(BACKWARD);
  }
  // If only sensor3 is on black line, turn right
  else if (sensor3 == LOW) {
    motor1.run(BACKWARD);
    motor2.run(FORWARD);
  }
  // If only sensor2 is on black line, move forward
  else if (sensor2 == LOW) {
    motor1.run(FORWARD);
    motor2.run(FORWARD);
  }
  // If all sensors are on white surface, move backward
  else {
    motor1.run(BACKWARD);
    motor2.run(BACKWARD);
  }
}

Подозреваю что библиотека не работает с данным Motor Shield https://amperka.ru/product/arduino-motor-shield