Raspberry PI 3 + OpenCV + Arduino UNO + ......(поледнее решение и вопрос в последнем сообщении)

Доброго времени суток! Вот и приближается время к разморозке ранее временно покинутого.
Собственно вопрос:
Кто имел опыт применения этого:
http://robot-develop.org/archives/3954
или чего-нибудь подобного?
Каковы мнения? И применял ли кто это:
DP-SLAM-master.zip
fastslam-master.zip

Конкретно интересует формат сохранения данных для применения и обращения к ним.
Спасибо!

 

Доброго времени суток!
Вот досада в одна и та же функция, точнее поток в разном режиме (при разных параметрах запуска)
работают по разному. Собственно поток:

Код (C++):

//тестовый поток
void    motorsProcessT(void)
{
    int    cntcntrl = 0;
    while(1)
    {
        timeMotorANDservo    = time(NULL);
        pthread_mutex_lock(&motorsMutex);
        cntcntrl = mcontrol(cntcntrl, &motors, &rdmotors);
        pthread_mutex_unlock(&motorsMutex);
        usleep(_wmotorsSleep);
    }
    pthread_exit(0);
}

в случае запуска с приёмом данных с консоли:

Код (C++):

//запуск тестовый потока
int    startworkT(char * serial, int baudrate, int symwait)
{
    int res;
    printf("---startworkT\n");
    res = MRTU_Connect(serial, baudrate, symwait);
    if(res != 0)
        return res;
    //mutex create
        if(!(pthread_mutex_init(&motorsMutex,NULL) == 0))
                return 4;
    printf("---create motorsProcessT\n");
    pthread_create(&hmotorsProcess,NULL,(void*) motorsProcessT, NULL);
    return 0;
}

работает нормально:

Код (Text):

голова влево +10

1 - колёса вперёд +10
2 - колёса назад  -10
3 - голова влево  +10
4 - голова вправо -10
5 - голова вверх  +10
6 - голова вниз   -10
0 - все позиции в 0
9 - выход
текущее состояние:
M1:0 M2:0 M3:0 M4:0 S1:20 S2:0 T:0
CMD:

т.е. состояния читаются из устройства и управление нормальное. А вот с приёмом данных через сервер
если создавать ещё один поток для работы с обменом без консоли:

Код (C++):

//запуск тестового потока через сервер
int    startClientTestWork(char * serial, int baudrate, int symwait)
{
    int res;
    res = MRTU_Connect(serial, baudrate, symwait);
    if(res != 0)
        return res;
    //mutex create
        if(!(pthread_mutex_init(&motorsMutex,NULL) == 0))
                return 4;
    //
    pthread_create(&hClientTestWork,NULL,(void*) ClientTestWork,NULL);
    pthread_create(&hmotorsProcess,NULL,(void*) motorsProcessT, NULL);
    return 0;
}

управление нормальное, а состояние ПШЫК!
(вот это уже в другой консоли через сервер)

Код (Text):

Состояние:
T:608477 Ml:0 Mr:0 Sh:0 Sv:0
Управление:
1 - левые приводы вперёд (+10)
2 - левые приводы назад (-10)
3 - правые приводы вперёд (+10)
4 - правые приводы назад (-10)
5 - голова влево (+10)
6 - голова вправо (-10)
7 - голова вверх (+10)
8 - голова вниз (-10)
0 - все приводы в "0"
9 - выход
5
 

при этом тестовый поток с сервером на самом устройстве

Код (C++):

//тестовый поток с передачей на сервер и приём статуса от сервера
void ClientTestWork(void)
{
    int res;
    int64_t    told    = 0;
    MOTORS* rm = (MOTORS*)malloc(sizeof(MOTORS));
    MOTORS*    wm = (MOTORS*)malloc(sizeof(MOTORS));
    //
    while(1)
    {
        res = ServerRdSet(&rdmovectl, &rdts);
        rm->motors[0] = rdmovectl.motorsL;
        rm->motors[2] = rdmovectl.motorsL;
        rm->motors[1] = rdmovectl.motorsR;
        rm->motors[3] = rdmovectl.motorsR;
        rm->servos[0] = rdmovectl.servoH;
        rm->servos[1] = rdmovectl.servoV;
        pthread_mutex_lock(&motorsMutex);
        memcpy(&motors, rm, sizeof(MOTORS));
        memcpy(wm, &rdmotors, sizeof(MOTORS));
        pthread_mutex_unlock(&motorsMutex);
        //memcpy(wm, rm, sizeof(MOTORS));
        printf("--- %i %i %i %i\n", rdmotors.motors[0], rdmotors.motors[1], rdmotors.servos[0], rdmotors.servos[1]);
        wrmovectl.motorsL = wm->motors[0];
        wrmovectl.motorsR = wm->motors[1];
        wrmovectl.servoH = wm->servos[0];
        wrmovectl.servoV = wm->servos[1];
        printf("=== %i %i %i %i\n", wrmovectl.motorsL, wrmovectl.motorsR, wrmovectl.servoH, wrmovectl.servoV);
        res = ServerWrStat(&wrmovectl, &rdts);
        usleep(_wmotorsSleep);
    }
    //pthread_mutex_unlock(&motorsMutex);
    free(rm);
    free(wm);  
    pthread_exit(0);
}

Во дела... ведь всё кроме обмена одно и то же
При чём если раскоментровать memcpy(wm, rm, sizeof(MOTORS)); то всё нормально - обмен через сервер проходит.

 

Пояснение:
в первом случае выходи по SSH на само устройство (там запущен клиент сонар и приводы в режиме управления через консоль)

второй случай это в консоли по SSH на второй малине со связью через сервер к первой малине.
вот статус на сервере:

Код (Text):

bot_gray@debian1:~/coding/bgds/work$ netstat -nltpa | grep 3132
(Not all processes could be identified, non-owned process info
will not be shown, you would have to be root to see it all.)
tcp        0      0 0.0.0.0:3132            0.0.0.0:*               LISTEN      1467/./bgds        
tcp        0      0 192.168.7.150:3132      192.168.7.102:47566     ESTABLISHED 1467/./bgds        
tcp        0      0 192.168.7.150:3132      192.168.7.101:45578     ESTABLISHED 1467/./bgds        
tcp        0      0 192.168.7.150:3132      192.168.7.102:47562     ESTABLISHED 1467/./bgds        
tcp        0      0 192.168.7.150:3132      192.168.7.101:45576     ESTABLISHED 1467/./bgds        
tcp        0      0 192.168.7.150:3132      192.168.7.102:47564     ESTABLISHED 1467/./bgds        
bot_gray@debian1:~/coding/bgds/work$

связь стоит как вкопанная! Но при этом управление проходит, а состояние нет. Сонар (дистанция)
в это же время нормально. Да и видео так же нормально.

 

Выкладывать исходники смогу только тогда, когда найду возможность делать ссылку на яндекс диск в шапке темы.
Если бы опциях темы кроме голосования было бы что-то полезное.

 

Доброго времени суток!
Уважаемые модераторы (и иные хозяева)! Если не затруднит сделайте возможность (Опции темы) делать ссылки и краткое описание, как на других сайтах. Это исключит необходимость каждый раз выкладывать код, который изменяется. Дабы не мусорить!

Заранее Спасибо!!!
С Глубоким Уважением!!!

 

Можно код в

Спойлер: Спойлеры

прятать

. Согласен с вами.

 

Доброго времени суток!
Это не то. Это только для визуального уменьшения сообщения! И я знаю что Вы понимаете о чем речь. А надо что бы как голосование сверху каждой страницы темы. Что бы не листать. А то мусор выходит... и сам путаешся, и все остальные уходят в ступор. А надо так - всё в шапке страницы... и изменение, и добавка, и прочее. А вставляемый код и ссылки внутри темы только для обсуждения.
Вот к примеру голосование на каждой странице, а не на первой.

 

Доброго времени суток!
Имея сонар кто и как измеряет скорость?
Имеем устройство на колёсиках... на нем малина с сонаром и приводами через ардуину (от той же малины). Малина по wi-fi соединена с сервером на PC(Debian). Программа сонар передаёт на сервер данные в виде структуры:

Код (C++):

typedef struct
{
    uint64_t    time;
    uint64_t    timeclock;
    int16_t        distance;
    int16_t        radius;
} TYPE_SONAR;

В которой время, время процессорное, дистанция и радиус (согласно диаграммы направленности сонара).
Другая малина из сервера читает по wi-fi из сервера эти данные и определяет скорость:

Код (C++):

float    MathSonarSpeed(TYPE_SONAR* ts, SONSPEED* ss, uint8_t mode)
{
    float    speed;
    float    dt = (float)(ts->timeclock - ss->OldTimeclock);
    if(!(dt > 0.0))
        dt = 0.000000001;
    switch(mode)
    {
        case _MathSonarSpeedMode_init:
            speed            = 0.0;
            ss->OldDist        = ts->distance;
            ss->OldTimeclock    = ts->timeclock;
            ss->Speed        = speed;
            break;
        case _MathSonarSpeedMode_f:
            speed = ((((float)(ss->OldDist - (ts->distance & (~(1)))) / dt) * 1000.0) + ss->Speed) / 2.0;
            ss->OldDist        = ts->distance;
            ss->OldTimeclock    = ts->timeclock;
            ss->Speed        = speed;
            break;
        case _MathSonarSpeedMode_nf:
        default:
            speed = ((float)(ss->OldDist - ts->distance) / dt) * 1000.0;
            ss->OldDist        = ts->distance;
            ss->OldTimeclock    = ts->timeclock;
            ss->Speed        = speed;
            break;
    }
    return speed;
}
 

для этого так же имеется структура:

Код (C++):

#define _MathSonarSpeedMode_nf     0x00   //без фильтра
#define _MathSonarSpeedMode_f     0x01   //с фильтром
#define _MathSonarSpeedMode_init   0x02   //инициализация структуры
typedef struct
{
   int64_t   OldTimeclock;
   float   Speed;
   int16_t   OldDist;
} SONSPEED;

собственно в тестовом режиме мы имеем:

Код (Text):

Скорость:0.00000
Скорость:1.63934
Скорость:0.41807
Скорость:0.20903
Скорость:0.10452
Скорость:0.05226
Скорость:0.02613
Скорость:0.01306
Скорость:0.00653
Скорость:0.00327
Скорость:0.00163
Скорость:0.00082
Скорость:0.00041
Скорость:0.00020
Скорость:0.00010
Скорость:0.00005
Скорость:0.00003
Скорость:0.00001
Скорость:0.00001
Скорость:0.00000
Скорость:0.78125
Скорость:0.11650
Скорость:0.05825
Скорость:0.02913
Скорость:0.01456
Скорость:0.00728
Скорость:0.00364
Скорость:0.00182
Скорость:0.00091
 

скрорость в см/с.
При этом принимаемая дистанция:

Код (Text):

- distance 66 radius 30 Tsonar:45485804 ------ "Cnrl+c" for exit
-- distance 66 radius 30 Tsonar:45485804 ------ "Cnrl+c" for exit
-- distance 66 radius 30 Tsonar:45485804 ------ "Cnrl+c" for exit
-- distance 65 radius 30 Tsonar:45486406 ------ "Cnrl+c" for exit
-- distance 65 radius 30 Tsonar:45486406 ------ "Cnrl+c" for exit
-- distance 65 radius 30 Tsonar:45486406 ------ "Cnrl+c" for exit
-- distance 65 radius 30 Tsonar:45486406 ------ "Cnrl+c" for exit
-- distance 65 radius 30 Tsonar:45486406 ------ "Cnrl+c" for exit
-- distance 65 radius 30 Tsonar:45486406 ------ "Cnrl+c" for exit
-- distance 65 radius 30 Tsonar:45486406 ------ "Cnrl+c" for exit
-- distance 65 radius 30 Tsonar:45486406 ------ "Cnrl+c" for exit
-- distance 65 radius 30 Tsonar:45486406 ------ "Cnrl+c" for exit
-- distance 65 radius 30 Tsonar:45486406 ------ "Cnrl+c" for exit
-- distance 66 radius 30 Tsonar:45487079 ------ "Cnrl+c" for exit

размерность: дистанция и радиус в сантиметрах, процессорное время милисекунды.
Следует отметить, что функция вызывается:

Код (C++):

while(1)
                {
                    pthread_mutex_lock(&ClientMutex);
                    printf("Скорость:%0.5f\n", MathSonarSpeed(&datasonar, &SonarSpeed, _MathSonarSpeedMode_f));
                    pthread_mutex_unlock(&ClientMutex);
                    usleep(250000); //usleep(_mainsleep);
                }

где:

Код (C++):

#define _MathSonarSpeedMode_nf        0x00    //без фильтра
#define _MathSonarSpeedMode_f        0x01    //с фильтром
#define _MathSonarSpeedMode_init    0x02    //инициализация структуры

Надо упомянуть, что данные с сервера читаются другим потоком в этой программе.

Кто и как избавляется от ошибок (младший бит - шум)? как выгоднее реализовать фильтр, при этом не снижая его быстродействия? Это важно - скорость параметр для поддержания задания для управления колёсами приводов (на первой малине) Собственно сама схема управления через сервер реализована.
Примечание:
Скорость и дистанция выводятся одной и той же программой (разные тесты) в разных консолях по SSH на второй малине
Заранее спасибо!

 

Вот изменил:

Код (C++):

float    MathSonarSpeed(TYPE_SONAR* ts, SONSPEED* ss, uint8_t mode)
{
    float    speed;
    float    dist;
    float    dt = (float)(ts->timeclock - ss->OldTimeclock);
    if(!(dt > 0.0))
        dt = 0.000000001;
    switch(mode)
    {
        case _MathSonarSpeedMode_init:
            speed            = 0.0;
            ss->OldDist        = ts->distance;
            ss->OldTimeclock    = ts->timeclock;
            ss->Speed        = speed;
            break;
        case _MathSonarSpeedMode_f:
            dist    = (float)(ss->OldDist - (int64_t)(ts->distance & (~(1))));
            speed    = ( dist / dt) * 1000.0;
            ss->OldDist        = (int64_t)(ts->distance & (~(1)));
            ss->OldTimeclock    = ts->timeclock;
            ss->Speed        = speed;
            break;
        case _MathSonarSpeedMode_nf:
        default:
            dist    = (float)(ss->OldDist - ts->distance);
            speed    = (dist / dt) * 1000.0;
            ss->OldDist        = ts->distance;
            ss->OldTimeclock    = ts->timeclock;
            ss->Speed        = speed;
            break;
    }
    return speed;
}

вот:

Код (Text):

Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
 

Конечно искажает дистанцию на 1 см, но для скорости пригодно. Вопрос теперь как отфильтровать для построения карты на плоскости? Тем более сонар поворачивать можно сервомашиной, но на колёсах энкодеров для движения устройства нет. В плане конструкции не реально (иначе надо собирать другое устройство). Может можно как то контролтровать перемещение гироскопом?

 

Но периодически проскакивает:

Код (C++):

Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:-1.46843
Скорость:1.50038
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000
Скорость:0.00000

Устройство при этом неподвижно.

 

С помощью навигации по уже увиденному? Мне кажется в гироскопе будет большая ошибка из-за вибрации и толчков.

 

Да наверное. Буду думать как обойтись без гироскопа и энкодера. Неудачную конструкцию наверное выбрал изначально. Придется ориентироваться по положению головы (сервомашины) и сонару на ней. Однозначно надо строить карту. Посмотрю в строну лидара сканирующего... возможно.

 

Доброго времени суток!
Вот пришлось к серверу подключиться программой (вместо второй малины):

тут подключение к серверу и состояния данных программ сонара и приводов.

Ну понятное дело всё надо проверить:

Тут тестовое правление и с поддержанием скорости и др. Если выставить флаг "Контроль скорости" то машинка будет поддерживать скорость вперёд. При дистанции < 30 см движение будет прекращено. Это касается и других режимов движения.

Ну и самое непонятное:

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

Шаг угла поворота сервомашинки по горизонтали при сканировании:

Код (C++):

int MathFuncHA(DSONAR *son)
{
        float   ld;
        float   aphi;
        //вычисление угла поворота
        ld = pow((pow(son->Dist, 2.0) + pow(son->Rad, 2.0)), 0.5);
        if(ld == 0.0)
                ld = 0.0000001;
        aphi = asin(son->Rad / ld) * (180.0 / M_PI);
        printf("son->Dist:%0.2f  son->Rad:%0.2f AngleH:%0.2f sphi:%0.2f\n", son->Dist, son->Rad, son->AngleH, aphi);
        return (int)aphi;
}

Потому как диаграмма направленности сонара на разных дистанциях разная.

Нанесение на координатную сетку исходя из угла и дистанции:

Код (C++):

void MathFinc1map(DSONAR *son, DMAP1 *map1)
{
    int x = _centrX;
    int y = _centrY;
    int x2,y2;
    //
    if(son->AngleH == 0)
    {
        x2 = son->Dist;
        y2 = 0;
    }
    else
    {
        x2 = son->Dist * sin((90 - son->AngleH) * M_PI / 180);
        y2 = sin(son->AngleH * M_PI / 180) * son->Dist;
    }
    x += x2;
    y += y2;
    printf("x=%i y=%i\n", x, y);
    if((x > 0) && (x < _szXDAarray) && (y > 0) && (y < _szYDAarray))
            if(map1->so[x][y] < 0xFF) map1->so[x][y] += 0x0F;
}

Собственно сейчас и отрабатываются пока.
Может кто-то подобное сам делал? Интересно! Может я что мудрю лишнего?
Спасибо!

 

Из-за низкого разрешения сонара (какой именно датчик вы используете?) имеет смысл рисовать битовую карту с расширением, например, 5 на 5 см.

 

Про сонар в этой теме. Разрешение 1 см. Ошибка сервомашины есть(зараза с шагом в 1 гр. не реагирует - визуально не наблюдаю). Шаг выбирается исходя из оговоренного пятна (про сонар в этой теме - к сожалению в шапке только голосование).
Функции не доделаны. В них применены теоремы "Пифагора" и "Синусов для треугольника". Пока только сканирование. Потом будет перемещение по карте при движении. Самое непонятное это поворот (нет ни энкодера, и другого). Надо будет разворачивать карту (её содержимое) относительно выбранной точки на ней. Но это полбеды. Нужна ориентация при повороте по содержимому карты. Заполнение пока приблизительное с инкрементом для повторяющихся позиций.
Массив:

Код (C++):

//структура локальной карты
#define _szXDAarray        481    //
#define _szYDAarray        481    //
#define    _centrX            (_szXDAarray / 2)
#define _centrY         (_szYDAarray / 2)
typedef struct
{
    uint8_t    so[_szXDAarray][_szYDAarray];    //
} DMAP1;

 

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

 

то, что увидел сонар и есть карта. Как вертеть массивы разберусь. А как узнать на сколько повернул в движении? Как без энкодера и пр? Я определяю скорость и дистанцию на какую переместился по сонару. Как определить поворот (разворот) когда включаю левые колёса вперёд, а правые назад (разворот вправо на месте)... определять соответствие после разворота и нового сканирования? ведь в реальности я не знаю на сколько повернул.

 

Доброго времени суток!
На сей раз опыт с перемещением:

Крестик - это машинка. Было сделано сканирование, после чего несколько перемещений. После каждого перемещения несколько сканирований. Модуль построения карты в доработке. Перезапись массива согласно дистанции перемещения соответственно:

Код (C++):

//движение вперёд
void MathFuncMove(DMAP1 *map1, int16_t dist)
{
    int cnt1,cnt2;
    int16_t d = dist;
    DMAP1 lmap1;
    memset(&lmap1, 0, sizeof(DMAP1));
    cnt1 = 0;
    while(cnt1 < _szXDAarray)
    {
        cnt2 = 0;
        while(cnt2 < _szYDAarray)
        {
            if(d < _szXDAarray)
            {
                lmap1.so[cnt1][cnt2] = map1->so[d][cnt2];
            }
            else
            {
                memcpy(map1, &lmap1, sizeof(DMAP1));
                return;
            }
            cnt2++;
        }
        d++;
        cnt1++;
    }
    memcpy(map1, &lmap1, sizeof(DMAP1));
}

 

Что-то шибко мелко, может увеличить размер пикселя карты. Вы сжимает фото перед загрузкой на форум?

 

Функция построения должна быть переделана... надо учитывать радиус пятна, на который происходит шаг сканирования. Размер пикселя не имеет значения - данная программа только для отработки алгоритма... ну и теста. Вместо этой программы другая малина в этой сети. Сейчас я её не включаю - отбирает управление первой малиной (на машинке).
По поводу картинок: надо сжимать? Как-то не обращал внимания. Сжимать - это картинка не в полный размер?
Я правильно понимаю? Буду делать маленькие.
Если разговор про формат, то он PNG.