Простой робот с датчиками
Материал из LicrymWiki
Итак, до этого мы собрали простого робота без датчиков, который ездит в случайных направлениях: Простой робот
Позже на портале был разработан простой ИК датчик препятствия:ИК бампер
Теперь было бы хорошо объединить эти две вещи. О том как работает датчик или робот подробно описано в предыдущих статьях, поэтому повторяться не будем.
[править] Аппаратная часть
Следует отметить что приведенные инструкции скорее всего нельзя повторить дословно, так как маловероятно что попадется такое же шасси, такие же детали, скорее всего будут отличия, например у вашего шасси электромотор будет потреблять меньше и дополнительный мост не потребуется, или вместо фототранзисторов как на схеме будут использованы аналоги. Рассматривайте текст только лишь как указание на общие принципы и идеи, воплощение которых процесс творческий, а результат индивидуальный. Итак, было собранно 6 ИК датчиков на маленьких платах с 4 контактным разъемом на каждой. Для оптической изоляции светодиода от фототранзистора на последние были надеты трубочки из черной изоленты. Датчики закреплены на шасси по 6 направлениям:
Вперед, F (номер 0)
Вперед налево, FL (номер 1)
Вперед направо, FR (номер 2)
Назад налево, BL (номер 3)
Назад, B (номер 4)
Назад направо, BR (номер 5)
В дальнейшем направления будут обозначаться именно так – одной или 2 буквами.
По схеме выводы 1,2,3 (питание, земля, включение светодиодов) соединены вместе. Выводы 4 (выход сигнала) заведены по каналам АЦП. Так как из-за разброса параметров деталей чувствительность датчиков может отличаться, то в программе введен массив ~gPorog, значения которого индивидуально задают порог срабатывания, что позволяет программно выровнять чувствительность. Так как тяговый мотор потребляет до 1,5 А при трогании (а микросхема-драйвер рассчитана всего на 0,6 А), то для него был отдельно сделан Н мост на ~MOSFET транзисторах.
В процессе написания программы обнаружился баг – не работали датчики, как в последствии оказалось – из-за тепловых шумов (см. процедуру опроса в коде). Для его поисков были установлены 6 зеленых светодиодов – как отладочный инструмент. Их установка необязательна.
[править] Программная часть
Программа робота является развитием программы робота Conqerror проекта Железный Феникс (http://www.ironfelix.ru/). В программе 2 таблицы вероятностей, одна рабочая, которая динамически корректируется, исходя из показаний датчиков, а вторая эталонная, ее значения соответствуют распределению вероятностей при отсутствии препятствий. В случае, если обнаруживается препятствие, то программа обнуляет колонку с направлением движения в сторону препятствия, распределяя ее вероятность поровну между разрешенными направлениями. Если препятствие исчезло, то программа восстанавливает запрещенное направление, но не сразу, а на некоторое количество шагов, заданных вначале. Это позволяет избежать дрожания перед препятствием.
Код:
/*****************************************************
This program was produced by the
CodeWizardAVR V1.25.7 beta 5 Professional
Automatic Program Generator
© Copyright 1998-2007 Pavel Haiduc, HP InfoTech s.r.l.
http://www.hpinfotech.com
Project :
Version :
Date : 02.03.2008 - 14.04.2008
Author : Spiritus Sancti
Company : licrym.org
Comments: CC BY-NC-SA
Chip type : ATmega8
Program type : Application
Clock frequency : 16,000000 MHz
Memory model : Small
External SRAM size : 0
Data Stack size : 256
*****************************************************/
#include <mega8.h>
#include <stdlib.h>
#include <delay.h>
#define ADC_VREF_TYPE 0x40
#define gTimeOfMove 976 //время элементарного хода в мсек (1 сек)
#define StepsOfRecover 4 //количество шагов, за которые восстанавливается направление
unsigned int gtime;
bit ADC_complete;
unsigned char sensor_state, sensor_state_last; /*переменная состояния датчиков,
соответствующий бит отвечает за соответствующий датчик*/
unsigned int gADC_result;
unsigned char now, h;
//расположение датчиков
//
// 1 0 2
// \ | /
// [ ]
// / | \
// 3 4 5
const unsigned char gporog[6]={30, 30, 30, 30, 30, 30}; /*массив индивидуальных порогов
срабатывания сенсоров*/
unsigned char d[7][7]={ /*таблица вероятностей переходов,
строки - текущее движение, столбцы - следующее.*/
// STO F FR FL B BR BL
/*STO*/ {10, 15, 15, 15, 15, 15, 15},
/*F */ {17, 23, 25, 25, 0, 0, 0},
/*FR */ {17, 23, 25, 25, 0, 0, 0},
/*FL */ {17, 33, 25, 25, 0, 0, 0},
/*B */ {25, 0, 0, 0, 25, 25, 25},
/*BR */ {25, 0, 0, 0, 25, 25, 25},
/*BL */ {25, 0, 0, 0, 25, 25, 25}
};
const unsigned char etalon[7][7]={ /*таблица вероятностей переходов,
образец из которого восстанавливать будем*/
// STO F FR FL B BR BL
/*STO*/ {10, 15, 15, 15, 15, 15, 15},
/*F */ {17, 23, 25, 25, 0, 0, 0},
/*FR */ {17, 23, 25, 25, 0, 0, 0},
/*FL */ {17, 33, 25, 25, 0, 0, 0},
/*B */ {25, 0, 0, 0, 25, 25, 25},
/*BR */ {25, 0, 0, 0, 25, 25, 25},
/*BL */ {25, 0, 0, 0, 25, 25, 25}
};
interrupt [TIM1_COMPA] void timer1_compa_isr(void) //прерывание по таймеру раз в 1/976 сек
{
if (gtime!=0) --gtime;
}
void delay (unsigned int time) //на вход задержка милисекундах, макс 65535
{
gtime=time;
while(gtime!=0){};
}
// ADC interrupt service routine
interrupt [ADC_INT] void adc_isr(void) //прерывание по завершению АЦП
{
unsigned int adc_data;
// Read the AD conversion result
adc_data=ADCW;
gADC_result=adc_data;
ADC_complete=1;
}
int poll_sensor(unsigned char SensorNumber) //процедура опроса сенсора
{
int delta, result1, result2;
ADMUX&=0b11110000; //сбрасываем биты микшера перед выбором входа
switch(SensorNumber) //включаем соответствующий вход
{
case 0:
ADMUX|=0b00000000;
break;
case 1:
ADMUX|=0b00000001;
break;
case 2:
ADMUX|=0b00000010;
break;
case 3:
ADMUX|=0b00000011;
break;
case 4:
ADMUX|=0b00000100;
break;
case 5:
ADMUX|=0b00000101;
break;
default:
ADMUX|=0b00000000;
break;
};
PORTD|=1<<5; //Светодиод на 5 ноге порта D
delay_us(50); //ждем 50 мкс пока светодиод разгорится
ADCSRA|=0x40; //записываем бит запускающий процесс измерения
while(ADC_complete!=1); //ждем пока не закончится измерение, проверяя флаг
result1=gADC_result;
ADC_complete=0; //сбрасываем флаг
PORTD&=~(1<<5); //выключаем светодиод и снова измеряем
delay_us(10);
ADCSRA|=0x40;
while(ADC_complete!=1);
result2=gADC_result;
delta=result2-result1; //Если вдруг из-за шумов у нас result2<result1 то
if (delta < 0) delta=0; /*получается отрицательное значение и сбой.
Это грабля.*/
ADC_complete=0;
return delta; //возвращаем значение разности
}
void show_sensors (void) /*вывести значения сенсоров на отладочные
светодиоды */
{
if(sensor_state & 0b00000001) PORTD|=1<<6; else PORTD&=~(1<<6);
if(sensor_state & 0b00000010) PORTD|=1<<7; else PORTD&=~(1<<7);
if(sensor_state & 0b00000100) PORTB|=1<<0; else PORTB&=~(1<<0);
if(sensor_state & 0b00001000) PORTB|=1<<1; else PORTB&=~(1<<1);
if(sensor_state & 0b00010000) PORTB|=1<<2; else PORTB&=~(1<<2);
if(sensor_state & 0b00100000) PORTB|=1<<3; else PORTB&=~(1<<3);
}
void check_env(void) //процедура проверки окружающего пространства
{
unsigned char i, a, b, c;
for(i=0; i<6; i++) //в цикле опросим все сенсоры
{
a=poll_sensor(i); //опрос сенсора 3 раза для исключения шума
b=poll_sensor(i);
c=poll_sensor(i);
if(c>=gporog[i]) c=1; else c=0;
if(b>=gporog[i]) b=1; else b=0;
if(a>=gporog[i]) a=1; else a=0;
c=c+b+a;
if(c>=2)sensor_state|=1<<i;
else sensor_state&=~(1<<i); /*если сенсор сработал то 1 в соотв бит
переменной состояний, если нет - то сбраcываем соотв бит*/
};
show_sensors();
}
void delay_walk (unsigned int time) //на вход задержка милисекундах, макс 65535,
{ /*схлапывающаяся задержка, обнуляется как только обнаруживается изменение показаний сенсоров*/
gtime=time;
while(gtime!=0)
{
check_env();
if (sensor_state != sensor_state_last) /*Если текущее состояние сенсоров необработано
- рвем задержку*/
{ //время проверки окружения 50 мкс (0,5мс)
break;
};
};
}
//далее функции управления двигателями
void f(int t) // Вперед
{
PORTD&=~(1<<1); //PORTD.1=0;
PORTD|=1<<2; //PORTD.2=1;
PORTD&=~(1<<3); //PORTD.3=0;
PORTD&=~(1<<4); //PORTD.4=0;
if(t!=0) delay_walk(t);
}
void b(int t) // назад
{
PORTD|=1<<1; //PORTD.1=1;
PORTD&=~(1<<2); //PORTD.2=0;
PORTD&=~(1<<3); //PORTD.3=0;
PORTD&=~(1<<4); //PORTD.4=0;
if(t!=0) delay_walk(t);
}
void fl(int t) // вперед налево
{
PORTD&=~(1<<1); //PORTD.1=0;
PORTD|=1<<2; //PORTD.2=1;
PORTD&=~(1<<3); //PORTD.3=0;
PORTD|=1<<4; //PORTD.4=1;
if(t!=0) delay_walk(t);
}
void fr(int t) // вперед направо
{
PORTD&=~(1<<1); //PORTD.1=0;
PORTD|=1<<2; //PORTD.2=1;
PORTD|=1<<3; //PORTD.3=1;
PORTD&=~(1<<4); //PORTD.4=0;
if(t!=0) delay_walk(t);
}
void bl(int t) // назад налево
{
PORTD|=1<<1; //PORTD.1=1;
PORTD&=~(1<<2); //PORTD.2=0;
PORTD&=~(1<<3); //PORTD.3=0;
PORTD|=1<<4; //PORTD.4=1;
if(t!=0) delay_walk(t);
}
void br(int t) // назад направо
{
PORTD|=1<<1; //PORTD.1=1;
PORTD&=~(1<<2); //PORTD.2=0;
PORTD|=1<<3; //PORTD.3=1;
PORTD&=~(1<<4); //PORTD.4=0;
if(t!=0) delay_walk(t);
}
void stop(int t) //стоп
{
PORTD&=~(1<<1); //PORTD.1=0;
PORTD&=~(1<<2); //PORTD.2=0;
PORTD&=~(1<<3); //PORTD.3=0;
PORTD&=~(1<<4); //PORTD.4=0;
if(t!=0) delay_walk(t);
}
void led_on(void) //Включить светодиод
{
PORTD|=1<<0; //PORTD.0=1;
}
void led_off(void) //Выключить светодиод
{
PORTD&=~(1<<0); //PORTD.0=0;
}
void go(unsigned char napr) /*Выборка направления и отдача команды
двигателям */
{
switch(napr)
{
case 0:
stop(gTimeOfMove);
break;
case 1:
f(gTimeOfMove);
break;
case 2:
fr(gTimeOfMove);
break;
case 3:
fl(gTimeOfMove);
break;
case 4:
b(gTimeOfMove);
break;
case 5:
br(gTimeOfMove);
break;
case 6:
bl(gTimeOfMove);
break;
};
}
void disable_way(unsigned char col) /*функция, запрещающая полученую колонку
таблицы*/
{
unsigned char zeroes, notZeroes, i, s, part;
for (s = 0; s <= 6; s++) //для каждой строки таблицы вероятностей
{
for (i=0; i<=6; i++) /*для текущей строки найдем кол-во нулей,
за исключением текущей ячейки заданной колонки*/
{ /*нуль - уже запрещенное направление,
мы должны найти количество пока еще разрешенных*/
if (d[s][i]==0 && i!=col) zeroes++; /*и найдя их количество раскидываем
вероятность текущей ячейки меж ними поровну*/
};
notZeroes = 6 - zeroes;
part = d[s][col] / notZeroes;
for (i=0; i<=6; i++)
{
if (d[s][i]!=0 && i!=col) d[s][col]+=part;
};
d[s][col]=0; //собственно само обнуление текущей ячеки
};
}
void enable_way (unsigned char col) /*функция восстановления значений таблицы
вероятностей*/
{unsigned char s;
for (s=0; s<=6; s++) //для каждой строки таблицы вероятностей
{ /*восстанавливаем табл. на (текущ -
знач.эталон)/(кол-во ходов для восстановления)*/
if (d[s][col] != etalon[s][col]) //за один раз
{
if (d[s][col] > etalon[s][col])
{
d[s][col]-= etalon[s][col] / StepsOfRecover;
};
if (d[s][col] < etalon[s][col])
{
d[s][col]+= etalon[s][col] / StepsOfRecover;
};
};
};
}
unsigned char next_move(void) //выясняем следующий ход
{
int a, b, c;
do
{
a = rand()/327; //0..99 число
c=0;
for (b=0; b<7; b++) //поик по таблице
{
if(a > c && a < (d[now][b]+c) ) break;
c=c+d[now][b];
};
}
while(b==7); /*Если сумма вероятностей в строке не 100,
то возможно пролететь */
/*всю таблицу без выбора и тогда b==7.
Если такое случается, то снова ищем но с новым случайным числом*/
now = b;
return b;
};
void decision(void) /*функция, которая будет принимать решения
на базе показаний датчиков*/
{
/* //Кусок кода, для распознавания препятствий по 6 направлениям
if(sensor_state & 0b00000010) disable_way(3); else enable_way(3); //FL sensor
if(sensor_state & 0b00000100) disable_way(2); else enable_way(2); //FR sensor
if(sensor_state & 0b00001000) disable_way(6); else enable_way(6); //BL Sensor
if(sensor_state & 0b00100000) disable_way(5); else enable_way(5); //BR Sensor
if(sensor_state & 0b00010000)
{
disable_way(4);
disable_way(5);
disable_way(6);
} else enable_way(4); //B sensor
if(sensor_state & 0b00000001)
{
disable_way(1);
disable_way(2);
disable_way(3);
} else enable_way(1); //F sensor
*/
//Кусок кода для распознавания препятствий по 2 направлениям
if (sensor_state & 0b00000001 || sensor_state & 0b00000010 || sensor_state & 0b00000100) //front
{
disable_way(1);
disable_way(2);
disable_way(3);
} else
{
enable_way(1);
enable_way(2);
enable_way(3);
};
if (sensor_state & 0b00010000 || sensor_state & 0b00001000 || sensor_state & 0b00100000) //back
{
disable_way(4);
disable_way(5);
disable_way(6);
} else
{
enable_way(4);
enable_way(5);
enable_way(6);
};
sensor_state_last = sensor_state;
}
void start(void) //начинаем движение
{
while(1)
{ /*проверяем окружение,
обрабатываем показания датчиков, получаем случайное направление и идем по нему*/
check_env();
decision();
h=next_move();
go(h);
led_on();
delay(100);
led_off();
};
}
void main(void)
{
int i;
// unsigned int delta;
// инициализация портов
PORTB=0x00;
DDRB=0xFF;
PORTC=0x00;
DDRC=0x00;
PORTD=0x00;
DDRD=0xFF;
TCCR0=0x00;
TCNT0=0x00;
// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 15,625 kHz
// Mode: Normal top=FFFFh
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer 1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: On
// Compare B Match Interrupt: Off
TCCR1A=0x00;
TCCR1B=0x0D;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x00;
OCR1AL=0x0F;
OCR1BH=0x00;
OCR1BL=0x00;
// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer 2 Stopped
// Mode: Normal top=FFh
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;
// External Interrupt(s) initialization
// INT0: Off
// INT1: Off
MCUCR=0x00;
// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x10;
// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;
// ADC initialization
// ADC Clock frequency: 1000,000 kHz
// ADC Voltage Reference: AVCC pin
ADMUX=ADC_VREF_TYPE & 0xff;
ADCSRA=0x8C;
// Global enable interrupts
#asm("sei")
while (1)
{
now=1;
for (i=0;i<=5;i++) /*Помигаем светодиодом после включения,
заодно задержка перед троганием что бы успеть отбежать*/
{
led_on();
delay(500);
led_off();
delay(500);
};
start();
};
}
