Для навигации хочу использовать пакеты gmapping, amcl, move_base. Теперь у меня вопрос - как скрестить ROS и программу в микроконтроллере? Я уже второй месяц тщетно бьюсь с этой программой, а защита уже 5 июня.. Очеень прошу кто разбирается, помогите пожалуйста.
Вот то что у меня пока получилось из программы для МК: (аргументы пока просто условные поставлены, т.к. я не понимаю в каком формате будет информация от ROS приходить на МК...
- Код: Выделить всё • Развернуть
#include <avr/io.h> //Библиотека ввода/вывода
#include <avr/interrupt.h> //Библиотека прерываний
#include <util/delay.h>
#define BAUDRATE 9600 // Скорость обмена данными
#define F_CPU 16000000UL // Рабочая частота контроллера
//Прототипы подпрограмм
void USART_Init( );
void USART_Byte_Transmit( unsigned char data );
void USART_Transmit( char* string );
void Forklift_Get( int item );
void Forklift_Release();
void Forklift_Stop();
//Подпрограмма-обработчик прерывания по приходу байта на модуль USART
ISR( USART0_RX_vect )
{
int data;
data = UDR0; //Присваиваем переменной b содержимое регистра UDR, в котором хранится принятый байт
if (data=='nomer')||(status==1) //если пришел номер груза для захвата, то говорим что погрузчик занят (т.к. status=1)
{
USART_Transmit("Forklift is busy. Press 2 to cancel current task!\r\n"); //Сообщение о занятости погрузчика, чтобы остановить задание
нажмите 2
data=0;
}
else if (data=='stop')
{
Forklift_Stop();
status=0; //Разрешаем начинать другие задания
USART_Transmit("Stopped\r\n"); //Сообщение об остановке
data=0;
}
}
void USART_Init( void ) //Инициализация модуля USART
{
/* Задаем скорость работы USART */
UBRR0H = (F_CPU/BAUDRATE/16-1)>>8;
UBRR0L = (F_CPU/BAUDRATE/16-1);
UCSR0B=(1<<RXEN)|( 1<<TXEN); // Разрешаем прием и передачу по USART
UCSR0B |= (1<<RXCIE); // Разрешаем прерывания по завершению приема
UCSR0C = (1<<USBS)|(3<<UCSZ0); //Устанавливаем формат данных 8 бит данных, 2 стоп бита
}
void USART_Byte_Transmit( unsigned char data ) //Функция отправки бита данных
{
while ( !(UCSR0A & (1<<UDRE)) ); //Ожидание опустошения буфера приема
UDR0 = data; //Начало передачи данных
}
void USART_Transmit( char* string ) //Функция отправки строки
{
while(*string != '\0')
{
USART_Byte_Transmit(*string);
string++;
}
}
void Forklift_Get( int item ) {
;//Функция захвата груза
}
void Forklift_Release() {
;//Функция опускания груза
}
void Forklift_Stop() {
;//Остановить погрузчик
}
int main(void) //Главная программа
{
USART_Init ();
Timer_Init();
status=0;
USART_Transmit("Ok!\r\n");//Передаем при включении
sei(); //разрешаем глобально прерывания
while(1) //вечный цикл
{
// Выполняем обработку принятого байта
while (data!=0)
{
if (data=='1')||(status==0) // Тут произвольно написана цифра 1, т.к. пока не знаю в каком формате передается информация о движении..
{
PORTX= (подаем питание на ножку ехать прямо) в каком виде при использовании пакета move_base передается информация на МК
чтоб управлять двигателем?
}
else if (data=='2')||(status==0)
{
PORTZ = (подаем питание на ножку повернуть влево)
}
//то же самое для "вправо"
}
while (data=nomer) //Когда приходит номер груза начинаем захват
{
Forklift_Get(data); //Взятие груза
USART_Transmit("Got it!\r\n"); //Сообщение об успешной идентификации груза
data=0;
}
while (data!=0)
{
if (data=='1')||(status==0) //Опять то же самое как и выше - едем только теперь в обратную сторону
{
PORTX= (подаем питание на ножку ехать прямо) в каком виде при использовании пакета move_base передается информация на МК
чтоб управлять двигателем?
}
else if (data=='2')||(status==0)
{
PORTZ = (подаем питание на ножку повернуть влево)
}
}
while (data=release) //Когда приходит сообщение, что надо опустить груз
{
Forklift_Release(); //Отпускаем груз
USART_Transmit("Done!\r\n"); //Сообщение об успешной доставке груза
data=0; //Сбрасываем переменную, чтобы погрузчик не поехал по второму разу
}
}
}
Как это объединить с пакетами навигации в ROS!?
