Робот "Объезжайка-1"

maksim
Offline
Зарегистрирован: 12.02.2012

И вот почти тот самый робот, который объезжает препятствия и пытается выехать из закрытого пространства. Пока еще только прототип робота на шилде и дуине с сыром кодом, но уже жизнеспособный. 

Основные материалы: Freeduino 2009, Motor Shield v3, ИК-дальномер SHARP 2Y0A02, ультразвуковой дальномер HC-SR04, Ni-MH аккумулятор 7.2V, мотор-редуктор Gekko MR12-100 (3шт.), Колеса Pololu 42x19 (пара), шаровая опора, кусок ПВХ.

Алгоритмы кода просты и не отточены, поэтому робот периодически не понимает куда можно ехать, а куда нет. Так же ИК-радар не всегда определяет очень темные или сильно отражающие предметы, а ультразвуковой дальномер иногда "не видит" плоскости под углом (на видео это дверь и кусок оргстекла около стены в конце видео). 

Код на всякий случай:

#include <AFMotor.h>

#define DIST_DIR 40  //
#define DIST_ANG 150 // 
#define SENS_BAL 80 // 

#define Trig_pin A4
#define Echo_pin A5
#define Radar_pin A0
#define LED 13

AF_DCMotor R_motor(3);
AF_DCMotor L_motor(4);

int M, R, L, S, IR_R, IR_L, ang, dist, ss = 0;
byte distant[148];
long dist_cm, s_r, s_l, s, ang_sum = 0;
boolean led, dir = 0;
volatile int angle = 0;
unsigned long millis_old_Ir, millis_old_Ult, millis_old_LED, millis_old_dir, micros_old_Ult = 0;

void setup(){
  Serial.begin(9600);
  pinMode(LED, OUTPUT);
  pinMode(Trig_pin, OUTPUT);
  digitalWrite(2, HIGH);
  attachInterrupt(0, circle, RISING);
}

void loop(){ 
  IR_radar();
  Ultrasound();

  if(millis()-millis_old_dir > 200){

    M = map(dist_cm, 5, DIST_DIR, -100, 50);
    M = constrain(M, -100, 50);

    ang = map(dist_cm, 0, DIST_ANG, 65, 20);
    ang = constrain(ang, 20, 65);

    ang_sum = ang_sum*4/5+ang;
    ang = ang_sum/5;
    
    ss = map(S, 40, SENS_BAL, 30, 50);
    ss = constrain(ss, 30, 50);

    int i = 0;
    while(i < ang){
      s_r += distant[82+i];
      s_l += distant[76-i];
      i++;
    }
    i = 0;
     while(i < 148){
      s += distant[i];
      i++;
    }
    S = s/148;
    IR_R = s_r/ss;
    IR_L = s_l/ss;
    s_r = 0;
    s_l = 0;
    s = 0;
    if(IR_L > IR_R){
      R = M + IR_L/5;
      L = M - IR_R/5;
    }
    else{
      R = M - IR_L/5;
      L = M + IR_R/5;
    }

    Motors(R, L);
    millis_old_dir = millis();
  }
  
  Led();
}

void Ultrasound(){
  if(millis()-millis_old_Ult > 100){
    digitalWrite(Trig_pin, HIGH);
    delayMicroseconds(4);
    digitalWrite(Trig_pin, LOW);
    micros_old_Ult = micros();
    while(!digitalRead(Echo_pin) && micros()-micros_old_Ult < 500){
    }
    micros_old_Ult = micros();
    while(digitalRead(Echo_pin) && micros()-micros_old_Ult < 20000){
    }
    dist_cm = (micros() - micros_old_Ult)/29.0/2;
    millis_old_Ult = millis();
  }
}

void IR_radar(){
  if(millis()-millis_old_Ir > 5){
    int sens = 14145/(analogRead(Radar_pin)+7);
    sens = min(sens, 150);
    distant[angle] = sens;
    angle++;
    angle = min(angle, 148);
    millis_old_Ir = millis();
  }
}

void circle() {
  angle = 0; 
  led = 1;
  millis_old_LED = millis();
}

void Motors(int R, int L){
  int speed_R = 0;
  int speed_L = 0; 
  R = constrain(R, -100, 100);
  L = constrain(L, -100, 100);

  if(R > 9){
    R_motor.run(FORWARD);
    speed_R = map(R, 10, 100, 100, 255);
  }
  else if(R < -9){
    R_motor.run(BACKWARD);
    speed_R = map(R, -10, -100, 100, 255);
  }
  else{
    R_motor.run(RELEASE);
  }
  R_motor.setSpeed(speed_R);

  if(L > 9){
    L_motor.run(FORWARD);
    speed_L = map(L, 10, 100, 100, 255);
  }
  else if(L < -9){
    L_motor.run(BACKWARD);
    speed_L = map(L, -10, -100, 100, 255);
  }
  else{
    L_motor.run(RELEASE);
  }
  L_motor.setSpeed(speed_L);
}

void Led(){
  if(led){
    digitalWrite(LED, led);
    if(millis()-millis_old_LED > 200){
      led = 0;
      digitalWrite(LED, led);
      Serial.print(S, DEC);
      Serial.print(" ");
      Serial.print(IR_R/2, DEC);
      Serial.print(" ");
      Serial.println(IR_L/2, DEC);
      millis_old_LED = millis();
    }
  }
}

 

Видео1 

Видео2

В общем робот будет еще дорабатываться...

"Arduino 2012"

Zapek@n
Offline
Зарегистрирован: 16.02.2012

 По-моему это просто здорово! Еще бы чуть чуть комментариев в код, а то с налету не очень понятно, но это так пожелание. В любом случае спасибо за код.

maksim
Offline
Зарегистрирован: 12.02.2012

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

Mastino
Offline
Зарегистрирован: 03.12.2011

Прикольный робот! У меня тоже похожий.

maksim пишет:

пытается выехать из закрытого пространства

А как он может пытаться если он сам не знает где сейчас находится?

maksim
Offline
Зарегистрирован: 12.02.2012

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

maksim
Offline
Зарегистрирован: 12.02.2012

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

И в зависимости от площадей этих секторов расчитывается направление и скорость (значение ШИМ) вращения правого и левого колес.

Mastino
Offline
Зарегистрирован: 03.12.2011

В общем это что то вроде:
if(left > right)
turnleft();
else if(right > left)
turnright();
else
goforward();

Так не интересно.. кинте взгляд на adaptive mapping. Думаю вас это заинтересует, и может со мною успехами поделитесь.. :)
Почему я пишу это, из ваших топиков в форуме видно что ваш уровень высокий! А у кого высокий уровень у того и требования высокие..:) Это я могу BLINK sketch`ом гордиться..:) а вы нет..:)

 

maksim
Offline
Зарегистрирован: 12.02.2012

Что-то вроде, но не так все просто, на данный момент вычисения выглядят так:

M = map(dist_cm, 5, DIST_DIRECTION, -100, 50); // Вычисление общих скорости и направления вращения по растоянию перед роботом
    M = constrain(M, -100, 50);
    
    M_sum = M_sum*4/5+M; // Сглаживание - фильтр резких изменений
    M = M_sum/5;

    ang = map(dist_cm, 0, DIST_ANGLE, 65, 20);  // Вычисление угла сегментов обзора радара по растоянию перед роботом
    ang = constrain(ang, 20, 65);

    ang_sum = ang_sum*4/5+ang;  // Сглаживание - фильтр резких изменений
    ang = ang_sum/5;

    int i = 0;
    while(i < ang){  // Подсчет средних значений растояния сегментов и превышений дальности радара
      s_r += distant[82+i];
      if(distant[82+i] == 150){
      r++;
      }
      s_l += distant[76-i];
      if(distant[76-i] == 150){
      l++;
      }
      i++;
    }
    
    i = 0;
    while(i < 148){ // Подсчет среднего значения растояния вокруг робота
      s += distant[i];
      i++;
    }

    p = map(dist_cm, 50, DIST_SENSETIVE, 5, 20); // Вычисление коэффициента усиления по растоянию перед роботом
    p = constrain(p, 5, 20);
    
    S = s/148; // Вычисление среднего значения растояния вокруг робота
    
    S_sum = S_sum*4/5+S;  // Сглаживание - фильтр резких изменений
    S = S_sum/5;

    ps = map(S, 40, 80, 20, 0); // Коректировка усиления по среднему значению растояния вокруг робота
    ps = constrain(ps, 0, 20);
        
    IR_R = s_r/ang/p+ps; // Расчет приращения для правой стороны
    IR_L = s_l/ang/p+ps; // Расчет приращения для левой стороны

    s_r = 0;
    s_l = 0;
    r = 0;
    l = 0;
    s = 0;

    if(IR_L+l > IR_R+r){
      R = M + IR_L;
      L = M - IR_R;
    }
    else{
      R = M - IR_L;
      L = M + IR_R;
    }

    Motors(R, L);

То что к этому сводится - да. Для начала я хочу добиться от робота более плавных и при этом четких траекторий движения. Картирование это уже второй вопрос и пока все то, что я видел из таких роботов двигались по траекториям с прямыми углами и очень медленно.

Mastino
Offline
Зарегистрирован: 03.12.2011

Пардон, не видел видео.. Объезжает правда круто! Плавно и красиво!
Теперь вопрос по проекту:
IR SHARP on крутится для красоты или нет? Как он понимает что измеряет дистанцию спереди а не сзади или наоборот?

 

maksim
Offline
Зарегистрирован: 12.02.2012

Вы же уже видили проект ИК-радар, если внимательно посмотреть там есть ик-датчик (ик-диод и фототранзистор в одном корпусе) и когда при вращении штырек припаяный к вращающейся части пересекает этот датчик, то срабатывает прерывание, так и определяется точка отсчета. Радар делает один оборот за 900млс +/-10млс давольно стабильная скорость вращения. Каждые 6млс дистанция записываются в массив.

Mastino
Offline
Зарегистрирован: 03.12.2011

А почему тогда в www.youtube.com/watch?v=-hZau6Z-CE4#t=1m45s IR не видит дверь по левой стороне и ориентируется только по пингу?

maksim
Offline
Зарегистрирован: 12.02.2012

Так как сонар не видит дверь, то угол обзора очень узкий и поэтому дверь под таким углом отражает ик-луч и радар тоже ее не видит. Когда робот подЪезжает ближе, то сонар всетаки засекает дверь, увеличивает угол обзора радара и отЪезжает назад, но как только отЪезжает теряет из вида дверь и опять уменьшает угол и едет на нее. То есть скорее всего радар не может увидить дверь пока ее не увидит сонар т.к. дверь при таком угле подЪезда к ней отражает и звук и свет, но иногда у робота получается развернуться от двери.

Mastino
Offline
Зарегистрирован: 03.12.2011

странно.. мой IR видеть под углом, не видеть только черных предметов, поэтому и спрашиваю. Когда начнете изучать и применять adeptive mapping?
 

maksim
Offline
Зарегистрирован: 12.02.2012

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

MAFia
Offline
Зарегистрирован: 26.10.2011

Как-то строил робота, об[твердый знак]езжающего препятствия. Только дальномера не было, использовал примитивизм из ИК-светодиодов и транзисторов. Об[твердый знак]езжал препятствия по такому-же алгоритму, только дальность ни к черту была, надо было вупор в препятствие в[твердый знак]езжал.

P.S. Почему при нажатии на [твердый знак], ничего не происходит, точнее переключаеться режим вкл/выкл?

maksim
Offline
Зарегистрирован: 12.02.2012

Опера с форумом не дружит, я ставлю так "обЪезд"

Евгений74
Offline
Зарегистрирован: 08.05.2012

maksim пишет:

 а для определения направления движения буду использовать компас.

А какой компас хотите использовать? Сам пока нашел вот такой http://www.goodluckbuy.com/3-axis-compass-module-hmc5883l-angle-module.html, но не знаю разберусь ли

maksim
Offline
Зарегистрирован: 12.02.2012

Пока не еще не заказал. Тот который вы предлагаете работает по i2c. Что бы не разбераться просто погуглите на него готовую библиотеку.

Mastino
Offline
Зарегистрирован: 03.12.2011

Компас нужно с компенсатором брать, иначе при малейшем наклоне показание компаса будут меняться.

maksim
Offline
Зарегистрирован: 12.02.2012

Mastino пишет:

странно.. мой IR видеть под углом, не видеть только черных предметов, поэтому и спрашиваю.

Если внимательно посмотреть видео, то видно что дверь и стол, даже под тем угом, что снимает камера отражают свет, что аж видно отражение работа и пола, поэтому и не видит их.

billybons2006
Offline
Зарегистрирован: 23.01.2013

А можно спросить по поводу конструкций вида:

M_sum = M_sum*4/5+M; // Сглаживание - фильтр резких изменений
M = M_sum/5;

Я для примера взял 5 замеров (интервал условно 1):
2;      1.5;     3;     10 (скачок); 3;    2.

Значение М получилось:
0.4;   0.62;   1.1;   2.88;            2.9;    2.72.

Эдакая грубоватая аппроксимация. Дальше считать не стал, т.к. стало ясно, что будет плавное огибание значений сверху/снизу.

Сильные скачки отрезаются (M = constrain(M, -100, 50)). Поэтому очень сильных отклонений не будет.

 

А то я для решения вопроса скачков задал таймер 2 секунды. Если обнаружил цель (ближе 30 см) снизил скорость, увеличил радиус поворота сервы, включил таймер. Цель пропала, но еще до истечения таймера буду ехать осторожно. Если цель перед носом (ближе 15 см), выполняю поворот с таймером, например, 1 секунда. Таким образом скачки замеров даже если и будут, они не влияют на общий режим движения.

 

Насколько я понимаю, ваш метод (*4/5, потом делим на 5) взят не эмпирически. Или это все-таки ваша придумка? Так многие делают?

maksim
Offline
Зарегистрирован: 12.02.2012

Этот способ когда-то подсказал step962, это способ усреднения без перехода на вещественные числа, но так как у вас и так вещественные числа, то смысла в таких манипуляциях нет. Не помню уже в какой теме это обсуждалось.

Russk1j
Offline
Зарегистрирован: 03.05.2012

Хорошая задумка.

maksim
Offline
Зарегистрирован: 12.02.2012

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

Russk1j
Offline
Зарегистрирован: 03.05.2012

Еще такой вопрос, нужно именно такая скорость вращения, или можно чуть медленнее и от этого суть не поменяется? И что за энкодер используете? Есть из принтера, хочу его попробовать использовать. Стабилизатор 7048 чтобы сохранить скорость вращения при подсаживании аккумуляторов?

maksim
Offline
Зарегистрирован: 12.02.2012

Конечно можно медленнее. Энкодер не помню откуда, обычные фототранзистор и ИК-светодиод. Не знаю что у вас за стабилизатор, у меня обычный линийный стабилизатор напряжения.

Russk1j
Offline
Зарегистрирован: 03.05.2012

У вас в теме ИК-радар на схеме показано питание 5В, питание идет прямо с платы? Аккумулятор я вижу последовательно соединен 2 и 3 или 4, получаем 6 или 7,2 В?

DielGamer
Offline
Зарегистрирован: 01.02.2014

при использовании кода, вылазит ошибка error: expected unqualified-id before numeric constant

 

 

m0rjjj
m0rjjj аватар
Offline
Зарегистрирован: 02.09.2013

Ощущение, что робот ищет где покакать =)

А робот клевый!