Робот "Объезжайка-1"
- Войдите на сайт для отправки комментариев
Пнд, 14/05/2012 - 20:03
И вот почти тот самый робот, который объезжает препятствия и пытается выехать из закрытого пространства. Пока еще только прототип робота на шилде и дуине с сыром кодом, но уже жизнеспособный.
Основные материалы: 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();
}
}
}
В общем робот будет еще дорабатываться...
"Arduino 2012"
По-моему это просто здорово! Еще бы чуть чуть комментариев в код, а то с налету не очень понятно, но это так пожелание. В любом случае спасибо за код.
Навряд-ли кому-то нужны и код и комментарии, если только так - для общего ознакомления с кодом, ведь такого же робота никто собирать не будет под этот код. Можно наверное было бы общие алгоритмы описать, но пока особо и описывать нечего.
Прикольный робот! У меня тоже похожий.
пытается выехать из закрытого пространства
А как он может пытаться если он сам не знает где сейчас находится?
Это не значит, что он "осознано" пытается выехать например из квартиры, он может выехать из комнаты т.к. когда он движится рядом с выходом из комноты, то там может оказаться больше свободного пространства и тогда он попытается именно туда и ехать.
Общий принцип - робот считает свободную площадь секторов справа и с лева от себя, причем углы секторов зависят от растояния перед роботом.
И в зависимости от площадей этих секторов расчитывается направление и скорость (значение ШИМ) вращения правого и левого колес.
В общем это что то вроде:
if(left > right)
turnleft();
else if(right > left)
turnright();
else
goforward();
Так не интересно.. кинте взгляд на adaptive mapping. Думаю вас это заинтересует, и может со мною успехами поделитесь.. :)
Почему я пишу это, из ваших топиков в форуме видно что ваш уровень высокий! А у кого высокий уровень у того и требования высокие..:) Это я могу BLINK sketch`ом гордиться..:) а вы нет..:)
Что-то вроде, но не так все просто, на данный момент вычисения выглядят так:
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);То что к этому сводится - да. Для начала я хочу добиться от робота более плавных и при этом четких траекторий движения. Картирование это уже второй вопрос и пока все то, что я видел из таких роботов двигались по траекториям с прямыми углами и очень медленно.
Пардон, не видел видео.. Объезжает правда круто! Плавно и красиво!
Теперь вопрос по проекту:
IR SHARP on крутится для красоты или нет? Как он понимает что измеряет дистанцию спереди а не сзади или наоборот?
Вы же уже видили проект ИК-радар, если внимательно посмотреть там есть ик-датчик (ик-диод и фототранзистор в одном корпусе) и когда при вращении штырек припаяный к вращающейся части пересекает этот датчик, то срабатывает прерывание, так и определяется точка отсчета. Радар делает один оборот за 900млс +/-10млс давольно стабильная скорость вращения. Каждые 6млс дистанция записываются в массив.
А почему тогда в www.youtube.com/watch?v=-hZau6Z-CE4#t=1m45s IR не видит дверь по левой стороне и ориентируется только по пингу?
Так как сонар не видит дверь, то угол обзора очень узкий и поэтому дверь под таким углом отражает ик-луч и радар тоже ее не видит. Когда робот подЪезжает ближе, то сонар всетаки засекает дверь, увеличивает угол обзора радара и отЪезжает назад, но как только отЪезжает теряет из вида дверь и опять уменьшает угол и едет на нее. То есть скорее всего радар не может увидить дверь пока ее не увидит сонар т.к. дверь при таком угле подЪезда к ней отражает и звук и свет, но иногда у робота получается развернуться от двери.
странно.. мой IR видеть под углом, не видеть только черных предметов, поэтому и спрашиваю. Когда начнете изучать и применять adeptive mapping?
Сейчас занимаюсь установкой еще одной пары колес, которые будут рулевыми и с энкодерами, т.к. колеса не ведущие, то пробуксовки практически исключены, думаю что смогу добиться более или менее нормальной точности по подсчету пройденного пути, а для определения направления движения буду использовать компас.
Как-то строил робота, об[твердый знак]езжающего препятствия. Только дальномера не было, использовал примитивизм из ИК-светодиодов и транзисторов. Об[твердый знак]езжал препятствия по такому-же алгоритму, только дальность ни к черту была, надо было вупор в препятствие в[твердый знак]езжал.
P.S. Почему при нажатии на [твердый знак], ничего не происходит, точнее переключаеться режим B вкл/выкл?
Опера с форумом не дружит, я ставлю так "обЪезд"
а для определения направления движения буду использовать компас.
А какой компас хотите использовать? Сам пока нашел вот такой http://www.goodluckbuy.com/3-axis-compass-module-hmc5883l-angle-module.html, но не знаю разберусь ли
Пока не еще не заказал. Тот который вы предлагаете работает по i2c. Что бы не разбераться просто погуглите на него готовую библиотеку.
Компас нужно с компенсатором брать, иначе при малейшем наклоне показание компаса будут меняться.
странно.. мой IR видеть под углом, не видеть только черных предметов, поэтому и спрашиваю.
А можно спросить по поводу конструкций вида:
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) взят не эмпирически. Или это все-таки ваша придумка? Так многие делают?
Этот способ когда-то подсказал step962, это способ усреднения без перехода на вещественные числа, но так как у вас и так вещественные числа, то смысла в таких манипуляциях нет. Не помню уже в какой теме это обсуждалось.
Хорошая задумка.
Да, везде использую такой способ усреднения, где можно обойтись целыми числами.
Еще такой вопрос, нужно именно такая скорость вращения, или можно чуть медленнее и от этого суть не поменяется? И что за энкодер используете? Есть из принтера, хочу его попробовать использовать. Стабилизатор 7048 чтобы сохранить скорость вращения при подсаживании аккумуляторов?
Конечно можно медленнее. Энкодер не помню откуда, обычные фототранзистор и ИК-светодиод. Не знаю что у вас за стабилизатор, у меня обычный линийный стабилизатор напряжения.
У вас в теме ИК-радар на схеме показано питание 5В, питание идет прямо с платы? Аккумулятор я вижу последовательно соединен 2 и 3 или 4, получаем 6 или 7,2 В?
при использовании кода, вылазит ошибка error: expected unqualified-id before numeric constant
Ощущение, что робот ищет где покакать =)
А робот клевый!