Блок кода обработки линейки датчиков линии (пост №9) из 5-и штук:
Все неопределенные тут переменные - глобалы из основного скетча движения по линии. Их значения должны сохраняться между вызовами loop().
Ошибка отклонения от линии считается как "среднее расстояние" всех датчиков на линии (что видят черное). Отсюда: важно иметь "средний или нулевой" датчик по центру линии. Его вес нулевой, но он участвует в количестве датчиков на линии, что позволяет отслеживать промежуточные значения от "веса" боковых датчиков +-1, +-3 и т.д.
Чтобы время нахождения датчиков четной ошибки и не четной было примерно одинаково, физическое расстояние между датчиками должно примерно соответствовать 2/3 от ширины линии. Это удобно для "узких" линий 15,20мм и не очень удобно для "русской" линии в 50мм.
Сохранение "предыдущих" значений в глобалах позволяет оперативно вычислять "радиус кривой" по которой сейчас едет тележка, что зная её вес, габариты, высоту центра тяжести и трение на колесах и передней опоре позволяет ТОЧНО вычислять максимально возможную скорость движения "конкретно сейчас" .. и быть "первым" по ТТХ моторов, веса и качества колес тележки.
P.S. Собственно это всё, что надо знать, чтобы быть первым. Отсюда: ПИД-регулятро не есть "айс" ..
C:
/**
* Модуль работы с датчиками линии
*
* @author Arhat109-20171009
*/
// количество усредняемых замеров
#define MAX_AVERAGE 3
// длительность в мсек. периода автонастройки уровней датчиков
#define MAX_AUTOLEVEL 10000
// всего датчиков линии. Оно же и ошибка направления - слева, + справа
// ВСЕГДА нечетное число .. важно!
#ifndef MAX_SENSORS
#define MAX_SENSORS 5
#endif
/** данные по датчикам 12 байт на датчик */
typedef struct _sensor_ {
uint8_t pin;
int8_t dir; // "расстояние" датчика от середины: <0 - влево, >0 - вправо
int black; // автонастройка: минимальное черное
int white; // автонастройка: максимальное белое
int lvl; // автонастройка: уровень между белым и черным;
int val; // замер
int adc; // результат преобразования в: 0 - белое, 1 - черное
} Sensors;
#define ptrSensor(p, n) (((Sensors *)(p))+n)
#ifndef sensor
Sensors sensor[MAX_SENSORS] = {
{ A7, -4, 1024, -1, 0, 0, 0}, // самый левый
{ A6, -2, 1024, -1, 0, 0, 0},
{ A5, 0, 1024, -1, 0, 0, 0},
{ A4, 2, 1024, -1, 0, 0, 0},
{ A3, 4, 1024, -1, 0, 0, 0}, // самый правый
};
#endif
#define snPin(p) (sensor[(p)].pin)
#define snBlack(p) (sensor[(p)].black)
#define snWhite(p) (sensor[(p)].white)
#define snVal(p) (sensor[(p)].val)
#define snLvl(p) (sensor[(p)].lvl)
#define snAdc(p) (sensor[(p)].adc)
#define snDir(p) (sensor[(p)].dir)
/**
* Вывод в монитор содержимого всех датчиков для проверок
*/
void printSensors()
{
for( uint8_t i=0; i<MAX_SENSORS; i++){
Serial.println();
Serial.print("s="); Serial.print(i, DEC);
Serial.print(", P="); Serial.print(snPin(i), DEC);
Serial.print(", D="); Serial.print(snDir(i), DEC);
Serial.print(", B="); Serial.print(snBlack(i), DEC);
Serial.print(", W="); Serial.print(snWhite(i), DEC);
Serial.print(", L="); Serial.print(snLvl(i), DEC);
Serial.print(", V="); Serial.print(snVal(i), DEC);
Serial.print(", A="); Serial.print(snAdc(i), DEC);
}
}
/**
* Начальная установка датчиков и зачистка данных по ним
*/
void setupSensors()
{
for( uint8_t i=0; i<MAX_SENSORS; i++ ){
pinMode( snPin(i), INPUT);
snBlack(i) = 1024;
snWhite(i) = -1;
snVal(i) = snLvl(i) = snAdc(i) = 0;
}
}
/** усреднение "бегущим средним" */
void getSensorsRun()
{
for( uint8_t n=MAX_AVERAGE; n; n--){
for( uint8_t i=0; i<MAX_SENSORS; i++){
snVal(i) = (analogRead( snPin(i) )*5 + snVal(i)*3) / 8; // к-ты подбирать по устойчивости, если надо!
snAdc(i) = (snVal(i) > snLvl(i)? 0 : 1);
}
}
}
/** усреднение арифметическое */
void getSensorsAvg()
{
static int adcs[MAX_SENSORS][MAX_AVERAGE];
int summ;
for( uint8_t n=MAX_AVERAGE; n; n--){
for( uint8_t i=0; i<MAX_SENSORS; i++){
adcs[i][n] = analogRead( snPin(i) );
}
}
for( uint8_t i=0; i<MAX_SENSORS; i++){
summ = 0;
for( uint8_t n=MAX_AVERAGE; n; n--){
summ += adcs[i][n];
}
snVal(i) = summ / MAX_AVERAGE;
snAdc(i) = (snVal(i) > snLvl(i)? 0 : 1);
}
}
/**
* Чтение всех датчиков "оптом"
* Усреднение удобно делать только на 4 замерах - деление заменяется свдигом!
*/
void getSensors()
{
getSensorsRun();
}
/** Расчет уровней белого, черного и среднего на каждый вызов */
void calcLevel()
{
getSensors();
for(uint8_t i=0; i<MAX_SENSORS; i++)
{
if( snVal(i) < snBlack(i) ){ snBlack(i) = snVal(i); }
if( snVal(i) > snWhite(i) ){ snWhite(i) = snVal(i); }
snLvl(i) = (snWhite(i) - snBlack(i))/2 + snBlack(i);
}
}
/**
* Процедура автонастройки уровня датчиков
* моргает всеми лампочками 2 раза быстро, настройка MAX_AUTOLEVEL сек, моргает 2 разf медленно
* Пока настраивается горит средний светодиод
*/
void autoLevel()
{
uint32_t time;
blinkYellow(2, 150, 350);
time = millis();
while( millis() - time < MAX_AUTOLEVEL){
blinkBlue(1,20,0);
calcLevel();
}
blinkYellow(2, 500, 500);
}
/**
* Проверяет как стоит машинка:
* Считаем среднее кроме среднего датчика и если:
* а) средний датчик показывает значительно меньше остальных, то он "на линии". Читаем уровни из EEPROM
* б) иначе считаем что "все датчики на белом" - запускаем автонастройку
* !!! Предполагается что все датчики показывают примерно одинаково на белом/черном !!!
*/
void getLevels()
{
int avg = 0;
int sensor0 = 0;
getSensors();
for( uint8_t i=0; i<MAX_SENSORS; i++){
if( i != (MAX_SENSORS)/2 ){
avg += snVal(i);
}
}
avg /= (MAX_SENSORS - 1);
sensor0 = snVal( (MAX_SENSORS)/2 );
//Serial.print(", sen0="); Serial.print((MAX_SENSORS)/2, DEC);
//Serial.print(", avg="); Serial.print(avg, DEC);
//Serial.print(", val0="); Serial.print(sensor0, DEC);
//Serial.println("");
if( abs(avg - sensor0 ) > avg * 3/8 ){
// стоим на линии, читаем EEPROM, моргаем средним и поехали:
blinkBlue(4, 350, 150);
readEEPROM(snEEPROM, (uint8_t *)sensor, sizeof(sensor));
}else{
// нет линии, моргаем крайними, настраиваем и сохраняем в EEPROM:
blinkRed(3, 350, 150);
autoLevel();
writeEEPROM(snEEPROM, sensor, sizeof(sensor));
}
}
/**
* Считает величину ошибки линии от середины машинки
* влево < 0, вправо > 0, ==0 -- линия посередине
*/
void calcDir()
{
int summ = 0;
int8_t count = 0;
// Serial.println("");
for( uint8_t i=0; i<MAX_SENSORS; i++ ){
summ += snAdc(i) * snDir(i);
count += snAdc(i);
if( snAdc(i) == 1 ){
digitalWrite(snLeds[i], HIGH); // датчик видит линию, включаем его светодиод
}else{
digitalWrite(snLeds[i], LOW); // не видит, выключаем.
}
}
// Serial.print("summ="); Serial.print(summ, DEC);
// Serial.print(", count="); Serial.print(count, DEC);
// смотрим что получилось: если есть датчики на линии, то ошибка = сумма расстояний поделить на число датчиков на линии
// иначе: линия потеряна и надо смотреть "где была" в прошлый замер
prevDir = dir;
if( count>0 ){
dir = summ/count;
}else{
if ( lastDir>0 ){ dir = MAX_SENSORS; } // линия была справа - потерялась там же..
else if( lastDir<0 ){ dir = -MAX_SENSORS; } // линия была слева ..
else{
// упс.. ехали прямо и потеряли линию .. продолжаем прямо
dir = 0;
}
}
// направление изменилось? нет: считаем сколько так уже едем, да: сохраняем текущее как "последнее" (last..), начинаем заново
if( prevDir == dir ){
dirCount++;
}else{
lastCount = dirCount;
lastDir = prevDir;
dirCount = 1;
}
// Serial.print(", prevDir="); Serial.print(prevDir, DEC);
// Serial.print(", dirCount="); Serial.print(dirCount, DEC);
// Serial.print(", lastDir="); Serial.print(lastDir, DEC);
// Serial.print(", lastCount="); Serial.print(lastCount, DEC);
// Serial.print(", dir="); Serial.print(dir, DEC);
}Ошибка отклонения от линии считается как "среднее расстояние" всех датчиков на линии (что видят черное). Отсюда: важно иметь "средний или нулевой" датчик по центру линии. Его вес нулевой, но он участвует в количестве датчиков на линии, что позволяет отслеживать промежуточные значения от "веса" боковых датчиков +-1, +-3 и т.д.
Чтобы время нахождения датчиков четной ошибки и не четной было примерно одинаково, физическое расстояние между датчиками должно примерно соответствовать 2/3 от ширины линии. Это удобно для "узких" линий 15,20мм и не очень удобно для "русской" линии в 50мм.
Сохранение "предыдущих" значений в глобалах позволяет оперативно вычислять "радиус кривой" по которой сейчас едет тележка, что зная её вес, габариты, высоту центра тяжести и трение на колесах и передней опоре позволяет ТОЧНО вычислять максимально возможную скорость движения "конкретно сейчас" .. и быть "первым" по ТТХ моторов, веса и качества колес тележки.
P.S. Собственно это всё, что надо знать, чтобы быть первым.
Отсюда: ПИД-регулятро не есть "айс" .. 
