BlockDuino: Робот не выезжающий за черную линию

четверг, 19 сентября 2013 г.

Робот не выезжающий за черную линию - Boundary Rider

Продолжаем экспериментировать с гусеничной платформой от Pololu.

Захотелось на этот раз сделать робота ездящего по линии, для этого я запаял датчик линии, вернее датчик яркости: Паяем датчик линии на CleClo:Proto
Но так как датчик пока один, то решил пока сделать робота не выезжающий за границу.
Робот состоит из:

Датчик подключил сразу напрямую в шину BlockDuino, благо этому ничего не мешает, шина у нас пустая. Подключил в порт A0.

Доступа в и-нет не было, поэтому алгоритм пришлось выдумывать самому. Хотя долго думать не пришлось, алгоритм простейший:

если под датчиком темно, то чуток отъезжаем назад и потом поворачиваем в заданную сторону на случайную величину
если под датчиком светло, то едем вперед и меняем направление поворота

Код под спойлером:

// Boundary Rider - Робот не выезжающий за черную границу
// http://blockduino.org
// http://blockduino.blogger.com/2013/09/boundary-rider.html
// Sept 2013

#include <BDMotor.h>     //для BlockDuino и BlockMotor
//#include <AFMotor.h>   //для Arduino и MotorShield

#define SPEED 255        // скорость вращения моторчиков, 255 - максимум
#define MIN_ROT 250      // минимальное время поворота, миллисекунд
#define BORDER 850       // показадния сенсора выше которого определяется черный цвет

#define ANALOG_PIN A0    // номер аналогового порта для считывания яркости пола
#define LED_PIN 13       // номер порта для ножки индикаторного светодиода

boolean isCW = true;     // переменная для хранения направления поворота 
int sensor;              // переменная для хранения показания сенсора
AF_DCMotor mL(1);        // #1 моторчик левой гусеницы
AF_DCMotor mR(2);        // #2 моторчик правой гусеницы


void setup() {
  pinMode(LED_PIN, OUTPUT); // Объявляем порт индикаторного светодиода
  
  mL.setSpeed(SPEED);    // устанавливаем скорость вращения левого моторчика
  mR.setSpeed(SPEED);    // устанавливаем скорость вращения правого моторчика
}

void loop() { // Основное тело программы
  sensor = analogRead(ANALOG_PIN);  // замеряем яркость пола
  if(sensor > BORDER) {             // если больше заданного, то это черный цвет
    digitalWrite(LED_PIN, HIGH);    // включаем индикаторный светодиод
    Back(MIN_ROT * 2);              // отъезжаем чуток назад
    Rotation(MIN_ROT * random(1, 10)); // поворачиваем на случайную величину
  } else {  // иначе (яркость пола меньше заданного - белый цвет)
    Run();                          // едем вперед
    isCW=!isCW;                     // меняем направление следующего поворота
    digitalWrite(LED_PIN, LOW);     // гасим индикаторный светодиод
  }    
} // конец основной программы, начинаем все сначала

//
// Подпрограммы
//
void Rotation(int t){  // подпрограмма Поворот
  if (isCW) TurnRight();   // если переменная isCW правда (true) то поворачиваем направо, 
    else TurnLeft();       //   иначе налево
  delay(t);                // ждем нужное время
  Stop();                  // останавливаемся
}

void Stop(){  // подпрограмма Остановка
      mL.run(RELEASE);
      mR.run(RELEASE);
}

void Run(){  // подпрограмма Ход вперед
      mL.run(FORWARD);
      mR.run(FORWARD);
}

void TurnRight(){ // подпрограмма Поворот направо
      mL.run(FORWARD);
      mR.run(BACKWARD);
}

void TurnLeft(){ // подпрограмма Поворот налево
      mR.run(FORWARD);
      mL.run(BACKWARD);
}

void Back(int t){ // подпрограмма Ход назад
      mR.run(BACKWARD);
      mL.run(BACKWARD);
      delay(t);   // ждем нужное время
      Stop();     // останавливаемся
}