Робот-сумо «Пятерочка».

собственно практически готовый робот "пятерочка"

скетч программы

//скетч для робота сумо с
// двумя сервомоторами DF15SR
//едет к препятствию, которое расположено  ближе чем distance_long и останавливается перед ним на расстоянии distance_short
//ускоренный вариант ( плптп не выдерживает скачков напряжения при остановке колес) - надо отдельное питание на плату

#include <Servo.h> //библиотека для управления серво - для поднятия и опускания платформы сумобота
#include <QTRSensors.h> // библиотека для подключения датчиков черной линии QTR8A

//QTR8A
#define NUM_SENSORS             2  // число используемых сенсоров черного и белого (первый и восьмой)
#define NUM_SAMPLES_PER_SENSOR  4  // average 4 analog samples per sensor reading
#define EMITTER_PIN             2  // подсветка на втором пине нано

Servo servo_r;  // создаем два объекта серво - правое и левое колесо
Servo servo_l;

// QTR8A - сенсоры 1 и 8 подключены к аналоговым выводам 0 и 1 ардуино нано
QTRSensorsAnalog qtra((unsigned char[]) {0, 1},
  NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];

const int valDelay = 100; //задержка в милисекундах для моторов (частота цикла loop без событий)

// определяем пины подключения для дистанционного сенсора
const int triggerPin = 11; // b3 на плате сенсоршилд для нано
const int echoPin = 12; // b4 на плате сенсоршилд для нано

const int distance_long = 45; // максимальная дистанция для поиска цели
const int distance_short = 3; // минимальная дистанция для цели (дистанция остановки перед целью)

const int wb_border_int = 600; //граница различения белого и черного - черный выше, белый ниже этой границы (от 0 до 1023)

const int low_motor_spd = 82; //86 стабильно
const int stop_motor_spd = 93;
const int fast_motor_spd = 104; //100 стабильно

long cm;                     // переменная для хранения дистанции в сантиметрах

void setup()
{
  // ШИМ у nano есть на выводах 3.5.6.9.10.11 - при помощи analogWrite() его можно использовать
  servo_r.attach(9);  // назначаем правому колесу вывод (b1) у шилда для нано
  servo_l.attach(10);  // левому колесу вывод (b2) у шилда для нано
  servo_r.write(stop_motor_spd);   // останавливаем сервы
  servo_l.write(stop_motor_spd);
  pinMode(triggerPin, OUTPUT); // датчик дистанции
  pinMode(echoPin, INPUT);
} 

void loop()
{ 

  qtra.read(sensorValues);              //читаем данные датчиков черного_белого и помещаем их в массив по количеству датчиков
  // проверяем, что робот находится не на белом фоне
  if((sensorValues [0] > wb_border_int) && (sensorValues [1] > wb_border_int)){

  cm = microsecondsToCentimeters();     // измеряем расстояние до объекта

  // едем прямо к препятствию, которое расположено ближе distance_long
  // вперед
   if ((cm <= distance_long) && (cm > distance_short)){
   move_forward();
   }

   if (cm > distance_long){
   // ищем препятствие , которое будет расположено на расстоянии distance_long и ближе крутясь на месте
   rotate_right();
   }

   // проверяем, не уперлись ли куда - то...
   if (cm <= distance_short)
   {
   stop_motors();
   }
   }
   else { // если датчики указывают на белое поле
   stop_motors();
   //отскочили подальше
   move_backward();
   delay(300);
   stop_motors();
   }
}
long microsecondsToCentimeters(){
   long duration;
   // The device is triggered by a HIGH pulse of 2 or more microseconds.
   // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
    digitalWrite(triggerPin, LOW);
    delayMicroseconds(2);
    digitalWrite(triggerPin, HIGH);
    delayMicroseconds(5);
    digitalWrite(triggerPin, LOW);  // The echo pin is used to read the signal from the device: a HIGH
                                    // pulse whose duration is the time (in microseconds) from the sending
                                    // of the ping to the reception of its echo off of an object.

  duration = pulseIn(echoPin, HIGH);
  // ***  THIS NEEDS TO BE CHECKED FOR THE HC-SR04  ***
  return duration / 29 / 2;
}

void move_forward(){
servo_r.write(low_motor_spd);
servo_l.write(fast_motor_spd);
delay(valDelay);
}

void stop_motors(){
servo_r.write(stop_motor_spd);   // останавливаем сервы
servo_l.write(stop_motor_spd);
delay(valDelay);
}

void move_backward(){
servo_r.write(fast_motor_spd);
servo_l.write(low_motor_spd);
delay(valDelay);
}

void rotate_right(){
servo_r.write(fast_motor_spd);
servo_l.write(fast_motor_spd);
delay(valDelay);

}

void rotate_left(){
servo_r.write(low_motor_spd);
servo_l.write(low_motor_spd);
delay(valDelay);
}