1503 linee diskr 2motora

Материал из УМКИwiki
Перейти к навигации Перейти к поиску
//////////////////////////////////////////////
//     2021.02.22 woronin,  umkiedu@gmail.com
//     Датчик линии цифровой сигнал, тестируем движение робота дискретно с регулировкой
//     Robot UMKI controller  K6_3
//     включаем правый и левый мотор регулируя скорость и путь, при езде по датчикам линии
//////////////////////////////////////////////
const int sensorL = A0; // Присоедниняем левый датчик к A0
const int sensorR = A1; // Присоедниняем правый датчик к A1
int Lsensor; // Переменная для левого датчика
int Rsensor; // Переменная для  правого датчика
int motorR1 = 2; //  направление вращения  первого правого мотора
int motorL1 = 4; //  направление вращения  первого левого мотора
int mLspeed = 3; // 3 управляет скоростью вращения  левого мотора
int mRspeed = 5; // 5 управляет скоростью вращения  правого мотора
// Задаем скорость вращения колес (от 0 до 255)
int V = 95;
//  время включения моторов в млсек
int timeM = 200;

void setup() // Задаем параметры инициализации переменных
{
  pinMode( sensorR , INPUT);// конфигурим  порт на вход
  pinMode( sensorL , INPUT);
  pinMode(mLspeed, OUTPUT); // конфигурим  порт на выход
  pinMode(mRspeed, OUTPUT);
  pinMode(motorR1, OUTPUT);
  pinMode(motorL1, OUTPUT);
  Serial.begin(9600); // Для отладки. Подключаем монитор порта на скорости общения 9600 байт в сек
}

// Основной бесконечный цикл
void loop() {
  Rsensor = digitalRead(sensorR); // считываем значение из порта в переменную
  Lsensor = digitalRead(sensorL);
  if ((Lsensor == 0) && (Rsensor == 0)) { // если оба датчика видят белый
    Serial.println("Lsensor =0 Rsensor=0"); //  печатаем отладочную информацию в монитор порта
    digitalWrite(motorR1, HIGH ); // правый мотор вперед
    analogWrite(mRspeed, V); //  старт правого мотора с заданной скоростью
    digitalWrite(motorL1 , HIGH ); // левый мотор вперед
    analogWrite(mLspeed, V); //  старт левого мотора с заданной скоростью
    delay(timeM);  // время включения в млсек
  }
  if ((Lsensor == 1) && (Rsensor == 0)) { // если  левый датчик видит черный
    Serial.println("Lsensor =1 Rsensor=0");
    digitalWrite( motorR1, HIGH ); // правый мотор вперед
    analogWrite(mRspeed, V); //  старт правого мотора с заданной скоростью
    digitalWrite( motorL1 , LOW ); // левый мотор Назад
    digitalWrite(mLspeed, HIGH); //  левого мотора на максимуме
    delay(timeM);
  }

  if ((Lsensor == 0) && (Rsensor == 1)) { // если  правый датчик видит черный
    Serial.println("Lsensor =0 Rsensor=1");
    digitalWrite( motorR1, LOW ); // правый мотор Назад
    analogWrite(mRspeed, V); //  старт правого мотора с заданной скоростью
    digitalWrite( motorL1 , HIGH ); // левый мотор вперед
    analogWrite(mLspeed, V); //  старт левого мотора с заданной скоростью
    delay(timeM);
  }

  if ((Lsensor == 1) && (Rsensor == 1)) { // если  оба датчика видит черный
    Serial.println("Lsensor =1 Rsensor=1");
    digitalWrite( motorR1, LOW ); // правый мотор Назад
    analogWrite(mRspeed, V); //  старт правого мотора с заданной скоростью
    digitalWrite( motorL1 , LOW ); // левый мотор Назад
    analogWrite(mLspeed, V); //  старт левого мотора с заданной скоростью
    delay(timeM);
  }

  digitalWrite(mRspeed, LOW); // скорость правого мотора стоп
  digitalWrite(mLspeed, LOW); // скорость левого мотора стоп
  delay(timeM);
}