Добавляем к роботу сенсоров

14 июня 2013, 12:40

Для того, чтобы робот мог объезжать препятствия, необходим специальный сенсор. Мы использовали ультразвуковой датчик расстояния. Под названием PING))) Ultrasonic Distance Sensor.

Принцип работы этого сенсора следующий: cенсор излучает короткий ультразвуковой импульс (в момент времени 0), который отражается от объекта и принимается сенсором. Расстояние рассчитывается исходя из времени до получения эха и скорости звука в воздухе.
Для подключения сенсора к Arduino мы использовали расширение Sensor Shield:

C помощью этого расширения очень удобно подключать к Arduino разные сенсоры и актуаторы (например сервомоторы). Кроме того, что на расширении есть все пины Arduino в обычном виде, как на самой плате, все они дополнительно выведены дополнительно и каждый пин дополнен двумя выводами питания «плюс» и «минус». Это позволят очень легко подключать сенсоры. Как правило все сенсоры имеют для подключения штекеры с тремя выводами «плюс»-«минус»-«сигнал»

На этой фотографии видно, как мы подсоединили к одной группе контактов ультразвуковой сенсор. Сам сенсор мы неподвижно закрепили в передней части робота.

Первая задача, которую необходимо решить — объезжать препятствия, встречающиеся роботу на пути, объезжая их либо справа, либо слева (без какой-либо логики).
Для получения сигналов с сенсора мы использовали библиотеку NewPing. Если расстояние до препятствия 10 сантиметров или меньше, робот пытается объехать его слева. При этом остается возможность управлять роботом с пульта.

Программа:

#include <IRremote.h>
#include <ArduMoto.h>
#include <NewPing.h>

int pingpin=4;  // Arduino pin tied to trigger pin on the ultrasonic sensor.
int maxdist=200 ;// Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

NewPing sonar(pingpin, pingpin, maxdist); // NewPing setup of pins and maximum distance.

ArduMoto Moto;  //Object to work with motors


int RECV_PIN = 7; //Pin with IR sensor

IRrecv irrecv(RECV_PIN); //object to work with IR sensor

decode_results results; //object to work with results of IR sensor 
const float kB=0.74 ; //коэффициент для мотора B, так как он крутится быстрее
const float kA=1;
const float kAll=0.7;//общий коэффициент

void setup()
{
  //Serial.begin(9600);
  irrecv.enableIRIn(); // подключаем ИК приемник
  Moto.begin();//подключаем моторы
  Moto.setSpeed('A',0);//останавливаем моторы на всякий случай
  Moto.setSpeed('B',0);

}
void left(int velocity)
{
  Moto.setSpeed('A',velocity*kAll*kA);
  Moto.setSpeed('B',-velocity*kAll*kB);
}

void right(int velocity)
{
  Moto.setSpeed('A',-velocity*kAll*kA);
  Moto.setSpeed('B',velocity*kAll*kB);
}


void forward(int velocity)
{
  Moto.setSpeed('A',velocity*kAll*kA);
  Moto.setSpeed('B',velocity*kAll*kB);
}

void backward(int velocity)
{
  Moto.setSpeed('A',-velocity*kAll*kA);
  Moto.setSpeed('B',-velocity*kAll*kB);
}
void forwardleft(int velocity)
{
  Moto.setSpeed('A',velocity*kAll*kA);
  Moto.setSpeed('B',0.4*velocity*kAll*kB);
}
void forwardright(int velocity)
{
  Moto.setSpeed('B',velocity*kAll*kB);
  Moto.setSpeed('A',0.4*velocity*kAll*kA);
}
void backwardright(int velocity){
  Moto.setSpeed('B',-velocity*kAll*kB);
  Moto.setSpeed('A',-0.4*velocity*kAll*kA);
}
void backwardleft(int velocity){
  Moto.setSpeed('A',-velocity*kAll*kA);
  Moto.setSpeed('B',-0.4*velocity*kAll*kB);
}

void loop() {      
  int distcm;
  distcm=sonar.ping_cm();//измеряем расстояние  в см.
  if( (distcm<=10)&&(distcm>0) ){//если до препятствия менее 10 см.
    forward(0);
    delay(50);
    backward(100);
    delay(100);
    left(100);
    delay(100);
    forward(100); 

  }
  if (irrecv.decode(&results)) {//если получили сигнал то надо что-то сделать
    switch (results.value){
    case 0x807fc03f://едем вперед
      forward(100);
      break;
    case 0x807f40bf:
      backward(100);
      break;
    case 0x807f9867:
      forward(0);
      break;
    case 0x807f708f:
      forwardleft(100);
      break;
    case  0x807f58a7:
      forwardright(100);
      break;
    case  0x807f807f:
      backwardright(100);
      break;
    case 0x807f06f9  :
      backwardleft(100);
      break;
    case 0x807f906f  :
      left(100);
      break;
    case 0x807fb847  :
      right(100);
      break;
    }
    irrecv.resume();
  }
}