Nonfinishism studious, заметки с тегом: Ultrasonic Sensor

Учим робота ориентироваться в пространстве. Робот, объезжающий препятствия.

Fri, 30 Aug 2013 11:26:52 +0300

Обычно говорят «Скоро сказка сказывается, да не скоро дело делается». В нашем случае все с точностью до наоборот. Этого робота мы закончили (ну или почти закончили) делать в мае, а «сказка» появляется только сейчас, почти через три месяца.
Долго ли, коротко ли, пришло время научить робота принимать какие-то осмысленные решения самостоятельно. Пока робот умеет только обнаруживать препятствия и объезжать их слева. Необходимо, чтобы он объезжал препятствие либо слева, либо справа, в зависимости от того, с какой стороны больше места для маневра. Если же робот заезжает в тупик (т. е. невозможно проехать ни вперед, ни вправо, ни влево), он должен задним ходом выехать из тупика.

Из всего вышеописанного следует, что нам необходимо не только контролировать расстояние до препятствия впереди, но и расстояние до препятствия слева и справа.
Вариантов несколько:

установить ультразвуковые сенсоры впереди, слева и справа,
сделать так, чтобы сеносор мог вращаться и смотреть в разные стороны.

Второй вариант немного сложнее, но кажется более правильным.
Для того, чтобы сенсор мог смотреть в разные стороны мы использовали сервопривод вот такой.

Сервопривод закрепили в передней части робота, на него поставили сенсор. Таким образом сенсор может смотреть вперед, поворачиваться на 90 градусов направо и налево.

Для подсоединения сервопривода используется трехконтактная колодка, как и для сенсора (плюс, минус и сигнал). Для подключения использовали Sensor Shield (на фотографии это хорошо видно).

Для того, чтобы робот смог объезжать препятствия был придуман такой алгоритм:

Робот все время мониторит свободное пространство перед собой.
Как только расстояние до препятствия становится меньше определенного минимального значения, робот останавливается.
Сенсор поворачивается на 90 градусов налево и измеряется расстояние до препятствия, затем на 90 градусов направо и тоже измеряет расстояние.
Робот должен повернуться в том направлении, где расстояние до ближайшего препятствия больше, при этом оно должно быть больше определенного минимального значения.
После завершения маневра поворота робот продолжает движение прямо, и алгоритм исполняется с пункта 1.
В случае если и справа и слева расстояние до препятствия меньше определенного минимального значения, робот включается задний ход. Алгоритм исполняется с пункта 3.

Для управления сервоприводом мы использовали стандартную библиотеку Arduino. В этом месте выяснился непрятный момент, что стандартная библиотека для сервопривода конфликтует с библиотекой IRremote.h, которую мы использовали для работы с инфракрасным приемником. Они обе используют один таймер (9-й пин), а второй подключить не прадставляется возможным (так как 3-й пин занят двигателем). В общем испробовали разные библиотеки для сервопривода, которые не используют таймер, но как-то нормально ни одна не заработала. В результате поисков мы набрели на очень интересную библиотеку, которая позволяет работать с разными устройствами, в т.ч. и с ИК приемником (http://www.zbotic.com).

Вот так это работает:

Здесь уже робота поджидают испытания по сложнее:

Программа:

#include "Device.h"
#include "PCInterrupt.h"
#include "irController.h"
#include "myHardware.h"

#include <Servo.h> 
#include <ArduMoto.h>
#include <NewPing.h>
int key;
IRController controller;

const int mindist=15;
const int pingpin=4;  // Arduino pin tied to trigger pin on the ultrasonic sensor.
const int maxdist=400 ;// Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
Servo servoping;
NewPing sonar(pingpin, pingpin, maxdist); // NewPing setup of pins and maximum distance.

ArduMoto Moto;  //создаем объект для работы с моторами

const float kB=0.55 ; //коэффициент для мотора B, так как он крутится быстрее
const float kA=1;
const float kAll=0.7;//общий коэффициент
const int delayServo=200;
int distcmAhead;
int distcmRight;
int distcmLeft;


void setup()
{

  servoping.attach(A0);
  Moto.begin();//подключаем моторы
  Moto.setSpeed('A',0);//останавливаем моторы на всякий случай
  Moto.setSpeed('B',0);
  servoping.write(90);
  Serial.begin(9600);
  int res = controller.begin(IR_PIN, OTHER_DEVICE);
  forward(0);
}

void stopall(){
  forward(0);
}

int wheretogo()
{
  if( (distcmLeft>=distcmRight)&&(distcmLeft>mindist)||(distcmLeft=0))//можем ли мы ехать налево
  {
    return 1;//go left     
  }

  else {
    if( (distcmLeft<distcmRight)&&(distcmRight>mindist)||(distcmRight=0))////можем ли мы ехать направо
    {
      return 2;//go right
    }
    else{
      return 3;//go back
    }
  }
}

int lookahead()
{
  servoping.write(90);
  delay(delayServo);
  int distcm=sonar.ping_cm();
  return distcm;
}

int lookleft(){
  servoping.write(180);
  delay(delayServo*2);
  int distcm=sonar.ping_cm();
  return distcm;

}
int lookright(){
  servoping.write(0);
  //SoftwareServo::refresh();
  delay(delayServo*2);
  int distcm=sonar.ping_cm();
  return distcm;
}

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)
{
  Serial.println('go forward');
  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 dir;
  distcmAhead=lookahead();
  if( (distcmAhead<=mindist)&&(distcmAhead>0)){
    stopall();
label1:   
    distcmLeft=lookleft();
    distcmRight=lookright();
    dir=wheretogo();
    switch (dir){
    case 1://едем налево
      left(100);
      delay(500);
      forward(100);
      break;
    case 2://едем направо
      right(100);
      delay(500);
      forward(100);
      break;
    case 3:
      backward(100);  
      do{
        distcmLeft=lookleft();
        distcmRight=lookright();
      }
      while( (distcmLeft<mindist)&&(distcmRight<mindist)&&(distcmRight>0)&&(distcmLeft>0));
      goto label1; 
    }
  } 


  key = controller.read();
  if (key>=0){//если получили сигнал то надо что-то сделать
    switch (key){
    case  16128://едем вперед
      forward(100);
      break;
    case 16192:
      backward(100);
      break;
    case   14726:
      forward(0);
      break;
    case  15427 :
      forwardleft(100);
      break;
    case   14662:
      forwardright(100);
      break;
    case   16256:
      backwardright(100);
      break;
    case  10200   :
      backwardleft(100);
      break;
    case  15746 :
      left(100);
      break;
    case  14471  :
      right(100);
      break;
    }
  }
}

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

Fri, 14 Jun 2013 11:40:02 +0300

Для того, чтобы робот мог объезжать препятствия, необходим специальный сенсор. Мы использовали ультразвуковой датчик расстояния. Под названием 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();
  }
}

Теперь робот умеет объезжать препятствия:

Небольшая фотосессия на вкусное: