Arduino.ru

#define SPEED_LEFT     6
#define SPEED_RIGHT    5
#define DIR_LEFT       7
#define DIR_RIGHT      4


#define SPEED         60
//float SPEEDp = 0, SPEEDm = 0 ;
float Kp = 25, Ki = 0, Kd = 0; //коэффициенты
float P = 0, I = 0, D = 0, PID_value = 0, previous_error = 0, R = 0, L = 0, err = 0;


// библиотека для работы с модулями I²C
#include <Wire.h>
// библиотека для работы cо сборкой датчиков линии
#include <Octoliner.h>

Octoliner octoliner(42); //обращение к датчику

void setup() 
{

  for (int i = 4; i <= 7; i++)
    pinMode(i, OUTPUT);

  Serial.begin(9600);
  // начало работы с модулями I²C
  Wire.begin();
  // начало работы с датчиками линии
  octoliner.begin();
  // выставляем чувствительность фотоприёмников в диапазоне от 0 до 255
  octoliner.setSensitivity(215);
  // выставляем яркость свечения ИК-светодиодов в диапазоне от 0 до 255
  octoliner.setBrightness(255);
}

void loop() {
R = 0;//датчики на белом с правой стороны 
L = 0;//датчики на белом с левой стороны 
int dataSensor[8];
  
  for (int i = 0; i < 8; i++) {
    dataSensor[i] = octoliner.analogRead(i);//переводим аналоговый сигнал в цифровой
  }
    for (int i = 0; i < 8; i++) {
      if (dataSensor[i] > 500)
      {
        dataSensor[i] = 1;
      }
      else
      {
        dataSensor[i] = 0;
      }
    }
   
  for(int i = 0; i < 8; i++) {//считаем скока датчиков на белом с левой стороны
    if (dataSensor[i] > 0)
    {
    L = L + 1;
    }
      else {
        L = 0;
      }
    }

    for(int i = 8; i < 0; i--) {//считаем скока датчиков на белом с правой стороны
    if (dataSensor[i] > 0)
    {
    R = R + 1;
    }
      else 
      {
        R = 0;
      }
    } 
  err = L - R;//вычисляем ошибку
  previous_error = err;//приведущая ошибка
  
  P = err;
  //I = I + err;
  D = err - previous_error;
  PID_value = (Kp * P) + (Kd * D) + (Ki * I);

  if (octoliner.analogRead(0) > 700) //защита
  {
    analogWrite(SPEED_LEFT, 100);
    analogWrite(SPEED_RIGHT, 100);
    digitalWrite(DIR_LEFT, LOW);
    digitalWrite(DIR_RIGHT, HIGH);
    while (octoliner.analogRead(3) < 700) {}
  }
  if (octoliner.analogRead(7) > 700) 
  {
    analogWrite(SPEED_LEFT, 100);
    analogWrite(SPEED_RIGHT, 100);
    digitalWrite(DIR_LEFT, HIGH);
    digitalWrite(DIR_RIGHT, LOW);
    while (octoliner.analogRead(4) < 700) {}
  }


  if ((SPEED - PID_value) > 0 && (SPEED + PID_value) > 0)//Езда
  {
    digitalWrite(DIR_LEFT, HIGH);
    digitalWrite(DIR_RIGHT, HIGH);
    analogWrite(SPEED_LEFT, constrain(SPEED -  PID_value, 0, 255));
    analogWrite(SPEED_RIGHT, constrain(SPEED + PID_value, 0, 255));
  }

  else if ((SPEED - PID_value) < 0)
  {
    digitalWrite(DIR_LEFT, LOW);
    digitalWrite(DIR_RIGHT, HIGH);
    analogWrite(SPEED_LEFT, (SPEED - PID_value) * -1);
    analogWrite(SPEED_RIGHT, SPEED + PID_value);
  }
  else if ((SPEED + PID_value) < 0)
  {
    digitalWrite(DIR_LEFT, HIGH);
    digitalWrite(DIR_RIGHT, LOW);
    analogWrite(SPEED_LEFT, SPEED - PID_value);
    analogWrite(SPEED_RIGHT, (SPEED + PID_value) * -1);
  }
  /*else if ((SPEED + err) > 0 && (SPEED - err) > 0)
    {
    digitalWrite(DIR_LEFT, HIGH);
    digitalWrite(DIR_RIGHT, HIGH);
    analogWrite(SPEED_LEFT, SPEED + err);
    analogWrite(SPEED_RIGHT, SPEED - err);
    }*/
}