2. Постановка задачи
Разработать программную систему, обеспечивающую в режиме реального времени:
- визуализацию среды функционирования, положения робота, целей и целевых траекторий движения робота, состояния процесса выполнения задач управления и всего моделирования в целом;
- отображение параметров, передаваемых сервером моделирования и параметров управления, передаваемых серверу моделирования;
- автоматическое управление движением робота для достижения целей задач управления роботом;
- интерфейс настройки параметров автоматического алгоритма управления роботом
3. Описание алгоритма работы программной системы
Программа написана на языке C++ с использованием .Net Framework 2.0 и двух библиотек: библиотеки GDI+ для работы с графикой и библиотеки WindowsSockets2 для сетевого взаимодействия.
В обработчике загрузки формы fmMain_Load() мы инициализируем работу с сетью и с библиотекой GDI+.
Форма нашей программной системы имеет 2 таймера. Таймер 1 изначально включен.
В обработчике тика таймера 1 timer1_Tick() происходит чтение глобальной переменной – структуры информации, полученной от сервера, и отображение значений её полей на форме. Также таймер 1 занимается визуализацией моделирования – отрисовкой на форме местоположения робота, оси координат и векторов скорости и силы тяги робота.
По нажатию пользователем на форме кнопки запуска процесса моделирования button_Start_Click() производится проверка возможности сетевого соединения с сервером, создается сокет для сетевого взаимодействия с сервером и происходит соединение. Если все действия выполнены успешно, то кнопка START становится недоступной, включается таймер 2 и создается дополнительный поток (он будет принимать от сервера данные о состоянии процесса моделирования и производить вычисление управляющего воздействия).
Функция работы дополнительного потока ThreadFunction(). В ней с использованием ранее установленного соединения и настроенного сокета в бесконечном цикле выполняются действия по приему от сервера строк специфицированного формата о текущем состоянии процесса моделирования. Прием осуществляется порциями размером до 128 байт (символов). Очередная принятая порция дописывается в буфер большого размера. После этого производится поиск символа ‘}’. Если символ не найден, то считываем дальше порции. Если найден, то создаем объект типа String и помещаем в него выделенную строку с информацией от сервера.
С помощью наших функций получаем численные значения всех параметров из объекта-строки. Вычисляем управляющее воздействие и записываем его в предназначенную для этого структуру.
В обработчике тика таймера 2 timer2_Tick() происходит отправка серверу строку со значениями управляющих воздействий – проекций вектора силы тяги на оси X и Y.
4. Листинг программы
#pragma once
ULONG_PTR gdiplusToken; // Для инициализации работы с графич. библиотекой GDI+.
Gdiplus::Bitmap* RobotBitmap; // Невидимое изображение, на к-ром все нарисуем, и к-рое потом выводим на форму.
Gdiplus::Graphics* BitmapGraphics; // Объект графики для рисования на невидимом изображении.
Gdiplus::Graphics* WindowGraphics; // Объект графики для рисования на форме.
//--
bool GlobalStop = false; // Флаг - при закрытии программы он сообщит созданному потоку, что пора умирать.
void* ThreadHandle = 0; // Хэндл созданного нами потока.
CRITICAL_SECTION Section; // Объект критическая секция.
//--
//wchar_t WideString[50];
char CharString[50];
//--
SOCKET RobotSocket = 0; // Сокет, с помощью к-рого будем общаться с сервером.
double Last_X=0, Last_Y=0; // Коор-ты предыдущей точки.
double ForceKoefficient = 2.4; // Коэффициент силы тяги.
//---------------------------------------------------------------------------
double StringToDouble(System::String^ I_String)
{
pin_ptr<const wchar_t> f_str = PtrToStringChars(I_String);
if (!f_str)
return 0.0;
wchar_t* f_end = ((wchar_t*)f_str) + I_String->Length;
return wcstod(f_str, &f_end);
}
//---------------------------------------------------------------------------
char* DoubleToString(double I_Double)
{
return _gcvt(I_Double, 15, CharString); // 15 - общее число цифр.
}
//---------------------------------------------------------------------------
unsigned long StringToULong(System::String^ I_String)
{
pin_ptr<const wchar_t> f_str = PtrToStringChars(I_String);
if (!f_str)
return 0;
wchar_t* f_end = ((wchar_t*)f_str) + I_String->Length;
return wcstoul(f_str, &f_end, 10);
}
//---------------------------------------------------------------------------
// Поиск подстроки в строке, между двух строк.
// Возвращает строку-результат. IO_Index - записывает новое смещение - откуда в след. раз начинать поиск.
System::String^ InsideString(System::String^ I_String, unsigned long& IO_Index, System::String^ I_Left, System::String^ I_Right)
{
System::String^ F_Result;
//---
pin_ptr<const wchar_t> f_string = PtrToStringChars(I_String);
pin_ptr<const wchar_t> f_left = PtrToStringChars(I_Left);
pin_ptr<const wchar_t> f_right = PtrToStringChars(I_Right);
//---
// Поиск подстроки - ЛЕВОЙ ГРАНИЦЫ. Возвращает адрес - начало этой подстроки.
const wchar_t* f_head = wcsstr(f_string + IO_Index, f_left);
if (f_head == 0)
{ // Не найдено такого.
IO_Index = -1;
return F_Result;
}
f_head += I_Left->Length; // Смещаемся вправо от начала лев. границы -> для попадания на начало искомой части.
//---
const wchar_t* f_tail = wcsstr(f_head, f_right); // Поиск правой границы.
if (f_tail == 0)
{ // Не найдено такого.
IO_Index = -1;
return F_Result;
}
//---
unsigned long f_length = f_tail - f_head; // Размер найденной строки.
wchar_t* f_buffer = new wchar_t[f_length+1]; // Выделяем память под нее.
wcsncpy(f_buffer, f_head, f_length); // Найденный кусок копируем -> в выдел. память.
f_buffer[f_length] = L'\0'; // В конце куска -> нуль-терминатор.
//---
F_Result = gcnew System::String(f_buffer); // Создаем объект-строку на основе найденной строки символов.
//---
IO_Index = I_Right->Length + (f_tail - f_string); // Для след. поиска - новый индекс.
//---
return F_Result;
}
//---------------------------------------------------------------------------
struct ServerInfo
{
double dT, dR, dM; // Текущ. время | Радиус робота | Текущ. масса робота.
double dRX, dRY; // Координаты робота.
double dVRX, dVRY; // Проекции скорости робота.
double dTR, dTX, dTY; // Для задачи 3: Радиус цели | Координаты цели.
char cTASK, cPOINT, cFAIL, cCRASH; // № задачи | Номер точки для задачи 2 | Флаг провала задачи | Флаг врезания в стену.
double dFUEL; // Текущий запас топлива.
char cDONE, cERROR, cIGNORED; // Флаг все задачи выполнены | Число неверных и проигнорированных сообщений от нас серверу.
};
struct ClientInfo
{
double dFRX, dFRY;
};
ServerInfo RobotServer;
ClientInfo RobotClient;
//---------------------------------------------------------------------------
// Функция работы потока.
// Бесконечно: Принимаем от сервера текущие параметры моделирования и Вычисляем управляющее воздействие.
unsigned long __stdcall ThreadFunction(void* I_Parameter)
{
SOCKET f_socket = (unsigned long)I_Parameter;
//---
char f_buffer[10000];
unsigned long f_length = 0; // Число в буфере занятых символов.
char f_sub[128];
while (1)
{
EnterCriticalSection(&Section);
bool f_exit = GlobalStop; // Читаем глоб. пер-ую - флаг того, что поток возможно уже должен умереть.
LeaveCriticalSection(&Section);
if (f_exit)
return 0; // Завершаем работу потока.
//---
// ПРИЕМ от сервера порции Д-х.
unsigned long f_size = recv(f_socket, f_sub, 128, 0);
if(f_size==0 || f_size==INFINITE) // Если 0, то сервер 100% больше ничего никогда не передаст, но может он нас чего-нибудь принять.
// Если -1 (INFINITE), то произошел разрыв соединения (программный или аппаратный).
{
MessageBoxW(0, L"Сервер разорвал соединение!!!", L"Ошибка!!!", MB_ICONERROR);
return 0;
}
//---
memcpy(f_buffer + f_length, f_sub, f_size); // Дописать в большой буфер полученную порцию.
f_length += f_size;
f_buffer[f_length] = '\0'; // Нуль-терминатор.
char* f_test = strchr(f_buffer, '}'); // Ф-ия поиска символа. Возвращает адрес символа, если тот там нашелся.
unsigned long f_index = (unsigned long)f_test; // Приводим указатель на символ к адресу в виде числа.
// Если не был найден символ '}'
if (f_index == 0)
continue; // Переход на следующую итерацию бесконеч. цикла.
//
// Символ '}' найден.
//
f_index -= ((unsigned long)f_buffer) - 1; // Получим отступ от начала буфера.
// Создаем строку и пишем в нее текст - целиком слово параметров от символа '{' до '}'.
System::String^ f_string = gcnew System::String(f_buffer, 0, f_index);
//---
f_buffer[f_index - 1] = '\0'; // Забить символ '}' значением 0, чтобы этот мусор не был найден.
// Копируем все накопленные (но еще не обработанные) в буфере данные ..
// .. и вставляем поверх уже прочитанных - в начало буфера.
// Освободившуюся часть в конце буфера считаем свободной для записи.
memcpy(f_buffer, f_buffer + f_index, f_length - f_index);
f_length -= f_index; // Размер занятости массива не забываем уменьшить.
//---
f_index = 0;
EnterCriticalSection(&Section);
RobotServer.dT = StringToDouble(InsideString(f_string, f_index, L"T=", L")"));
RobotServer.dR = StringToDouble(InsideString(f_string, f_index, L"R=", L")"));
RobotServer.dM = StringToDouble(InsideString(f_string, f_index, L"M=", L")"));
RobotServer.dRX = StringToDouble(InsideString(f_string, f_index, L"RX=", L")"));
RobotServer.dRY = StringToDouble(InsideString(f_string, f_index, L"RY=", L")"));
RobotServer.dVRX = StringToDouble(InsideString(f_string, f_index, L"VRX=", L")"));
RobotServer.dVRY = StringToDouble(InsideString(f_string, f_index, L"VRY=", L")"));
RobotServer.dTR = StringToDouble(InsideString(f_string, f_index, L"TR=", L")"));
RobotServer.dTX = StringToDouble(InsideString(f_string, f_index, L"TX=", L")"));
RobotServer.dTY = StringToDouble(InsideString(f_string, f_index, L"TY=", L")"));
RobotServer.cTASK = StringToULong(InsideString(f_string, f_index, L"TASK=", L")"));
RobotServer.cPOINT = StringToULong(InsideString(f_string, f_index, L"POINT=", L")"));
RobotServer.cFAIL = StringToULong(InsideString(f_string, f_index, L"FAIL=", L")"));
RobotServer.cCRASH = StringToULong(InsideString(f_string, f_index, L"CHASH=", L")"));
RobotServer.dFUEL = StringToDouble(InsideString(f_string, f_index, L"FUEL=", L")"));
RobotServer.cDONE = StringToULong(InsideString(f_string, f_index, L"DONE=", L")"));
RobotServer.cERROR = StringToULong(InsideString(f_string, f_index, L"ERROR=", L")"));
RobotServer.cIGNORED = StringToULong(InsideString(f_string, f_index, L"IGNORED=", L")"));
//---
if (RobotServer.cFAIL || RobotServer.cCRASH)
{
LeaveCriticalSection(&Section);
return 0; // Провалили задание или врезались в стену. ВЫХОД.
}
//---
if(RobotServer.cTASK==2)
switch(RobotServer.cPOINT)
{
case 0: RobotServer.dTX=0.7; RobotServer.dTY=0; Last_X=0; Last_Y=0; break;
case 1: RobotServer.dTX=0; RobotServer.dTY=0.7; Last_X=0.7; Last_Y=0; break;
case 2: RobotServer.dTX=-0.3; RobotServer.dTY=0; Last_X=0; Last_Y=0.7; break;
case 3: RobotServer.dTX=-0.7; RobotServer.dTY=0; Last_X=-0.3; Last_Y=0; break;
case 4: RobotServer.dTX=0.7; RobotServer.dTY=0; Last_X=-0.7; Last_Y=0; break;
case 5: RobotServer.dTX=0; RobotServer.dTY=-0.8; Last_X=0.7; Last_Y=0; break;
case 6: RobotServer.dTX=0; RobotServer.dTY=-0.8; Last_X=0.0; Last_Y=-0.8; break;
}
//------------------------------- ФОРМУЛА --------------------------------------
double f_force_koef = ForceKoefficient; // 2.4
//
if ((fabs(RobotServer.dRX-RobotServer.dTX)<0.1) && (fabs(RobotServer.dRY-RobotServer.dTY)<0.1))
f_force_koef *= RobotServer.dM / 80.0;
//---
RobotClient.dFRX = f_force_koef * ((RobotServer.dRX - RobotServer.dTX) + RobotServer.dVRX);
RobotClient.dFRY = f_force_koef * ((RobotServer.dRY - RobotServer.dTY) + RobotServer.dVRY);
//------------------------------------------------------------------------------
LeaveCriticalSection(&Section);
}
//---
return 0;
}
//---------------------------------------------------------------------------
namespace Robot2 {
using namespace System;
using namespace System::ComponentModel;
using namespace System::Collections;
using namespace System::Windows::Forms;
using namespace System::Data;
using namespace System::Drawing;
public ref class Form1 : public System::Windows::Forms::Form
{
public:
Form1(void)
{
InitializeComponent();
}
protected:
~Form1()
{
if (components)
{
delete components;
}
}
// Обработчик события создания формы.
private: System::Void fmMain_Load(System::Object^ sender, System::EventArgs^ e)
{
InitializeCriticalSection(&Section);
//---
WSADATA f_wsadata;
WSAStartup(MAKEWORD(2, 2), &f_wsadata); // Инициализируем сеть.
//---
Gdiplus::GdiplusStartupInput gdiplusStartupInput;
Gdiplus::GdiplusStartup(&gdiplusToken, &gdiplusStartupInput, NULL); // Инициализируем GDI+.
//---
RobotBitmap = new Gdiplus::Bitmap(532, 496, PixelFormat32bppRGB);
BitmapGraphics = new Gdiplus::Graphics(RobotBitmap);
WindowGraphics = new Gdiplus::Graphics((HWND)panel_Video->Handle.ToPointer(), (int)0);
}
// Обработчик нажатия кнопки "Start".
private: System::Void button_Start_Click(System::Object^ sender, System::EventArgs^ e)
{
ADDRINFOW f_hints, *f_addrinfo;
memset(&f_hints, 0, sizeof(ADDRINFOW));
f_hints.ai_family = PF_INET; // Задать, что это IP.
f_hints.ai_socktype = SOCK_STREAM; // И это TCP.
//---
pin_ptr<const wchar_t> f_host = PtrToStringChars(textBox_Host->Text); // Получаем с формы значение IP-адреса.
// Ф-ия по сети проверит наличие сервера с заданными пар-рами.
if (GetAddrInfoW(f_host, L"9999", &f_hints, &f_addrinfo)) // Получили в "f_addrinfo" - структуру для соединения по сети.
{
MessageBoxW(0, L"Произошла ошибка с сетью!!!", L"Ошибка!!!", MB_ICONERROR);
return;
}
//---
if (f_addrinfo->ai_family != f_hints.ai_family) // Проверить на валидность (совпадение того, что есть и того, что хотели)
{
FreeAddrInfoW(f_addrinfo);
MessageBoxW(0, L"Произошла ошибка с сетью!!!", L"Ошибка!!!", MB_ICONERROR);
return;
}
//---
// Создаем сокет для соединения с сервером.
RobotSocket = socket(f_addrinfo->ai_family, f_addrinfo->ai_socktype, f_addrinfo->ai_protocol); // Создать сокет (IP, TCP, Protocol)
if (RobotSocket == INVALID_SOCKET)
{
FreeAddrInfoW(f_addrinfo);
MessageBoxW(0, L"Произошла ошибка с сетью!!!", L"Ошибка!!!", MB_ICONERROR);
return;
}
//---
// Соединяемся с сервером.
if (connect(RobotSocket, f_addrinfo->ai_addr, f_addrinfo->ai_addrlen) == SOCKET_ERROR) // Подключить сокет к серверу
{
closesocket(RobotSocket);
FreeAddrInfoW(f_addrinfo);
MessageBoxW(0, L"Произошла ошибка с сетью!!!", L"Ошибка!!!", MB_ICONERROR);
return;
}
//---
// СОЕДИНИЛИСЬ С СЕРВЕРОМ.
//---
FreeAddrInfoW(f_addrinfo); // Очистить память
//---
GlobalStop = false; // Флаг для потока, работать дальше или сдохнуть.
unsigned long f_id;
// Создаем новый поток.
ThreadHandle = CreateThread(0, 0, ThreadFunction, (void*)RobotSocket, 0, &f_id); // Создать нить и получить ее ID.
if (!ThreadHandle)
{
closesocket(RobotSocket);
MessageBoxW(0, L"Произошла ошибка с нитью!!!", L"Ошибка!!!", MB_ICONERROR);
return;
}
//---
button_Start->Enabled = false;
timer2->Enabled = true; // ВКЛЮЧАЕМ таймер 2.
//---
panel_Video->Focus();
}
// Обработчик события закрытия формы.
private: System::Void fmMain_FormClosing(System::Object^ sender, System::Windows::Forms::FormClosingEventArgs^ e)
{
EnterCriticalSection(&Section);
GlobalStop = true; // Устанавливаем флаг для нашего потока, чтоб он умер.
LeaveCriticalSection(&Section);
//---
WaitForSingleObject(ThreadHandle, INFINITE); // Ждем, пока поток умрет.
closesocket(RobotSocket);
//---
WSACleanup(); // Выйти из сети.
//---
delete WindowGraphics;
delete BitmapGraphics;
delete RobotBitmap;
//---
Gdiplus::GdiplusShutdown(gdiplusToken); // Выйти из GDI+.
}
// Обработчик Таймера 1.
private: System::Void timer1_Tick(System::Object^ sender, System::EventArgs^ e)
{
System::String^ f_info;
EnterCriticalSection(&Section);
f_info+=gcnew System::String(L"T="); f_info+=gcnew System::String(DoubleToString(RobotServer.dT)); f_info+=gcnew System::String(L"\r\n");
f_info+=gcnew System::String(L"R="); f_info+=gcnew System::String(DoubleToString(RobotServer.dR)); f_info+=gcnew System::String(L"\r\n");
f_info+=gcnew System::String(L"M="); f_info+=gcnew System::String(DoubleToString(RobotServer.dM)); f_info+=gcnew System::String(L"\r\n");
f_info+=gcnew System::String(L"RX="); f_info+=gcnew System::String(DoubleToString(RobotServer.dRX)); f_info+=gcnew System::String(L"\r\n");
f_info+=gcnew System::String(L"RY="); f_info+=gcnew System::String(DoubleToString(RobotServer.dRY)); f_info+=gcnew System::String(L"\r\n");
f_info+=gcnew System::String(L"VRX="); f_info+=gcnew System::String(DoubleToString(RobotServer.dVRX)); f_info+=gcnew System::String(L"\r\n");
f_info+=gcnew System::String(L"VRY="); f_info+=gcnew System::String(DoubleToString(RobotServer.dVRY)); f_info+=gcnew System::String(L"\r\n");
f_info+=gcnew System::String(L"TR="); f_info+=gcnew System::String(DoubleToString(RobotServer.dTR)); f_info+=gcnew System::String(L"\r\n");
f_info+=gcnew System::String(L"TX="); f_info+=gcnew System::String(DoubleToString(RobotServer.dTX)); f_info+=gcnew System::String(L"\r\n");
f_info+=gcnew System::String(L"TY="); f_info+=gcnew System::String(DoubleToString(RobotServer.dTY)); f_info+=gcnew System::String(L"\r\n");
f_info+=gcnew System::String(L"TASK="); f_info+=gcnew System::String(DoubleToString(RobotServer.cTASK)); f_info+=gcnew System::String(L"\r\n");
f_info+=gcnew System::String(L"POINT="); f_info+=gcnew System::String(DoubleToString(RobotServer.cPOINT)); f_info+=gcnew System::String(L"\r\n");
f_info+=gcnew System::String(L"FAIL="); f_info+=gcnew System::String(DoubleToString(RobotServer.cFAIL)); f_info+=gcnew System::String(L"\r\n");
f_info+=gcnew System::String(L"CRASH="); f_info+=gcnew System::String(DoubleToString(RobotServer.cCRASH)); f_info+=gcnew System::String(L"\r\n");
f_info+=gcnew System::String(L"FUEL="); f_info+=gcnew System::String(DoubleToString(RobotServer.dFUEL)); f_info+=gcnew System::String(L"\r\n");
f_info+=gcnew System::String(L"DONE="); f_info+=gcnew System::String(DoubleToString(RobotServer.cDONE)); f_info+=gcnew System::String(L"\r\n");
f_info+=gcnew System::String(L"ERROR="); f_info+=gcnew System::String(DoubleToString(RobotServer.cERROR)); f_info+=gcnew System::String(L"\r\n");
f_info+=gcnew System::String(L"IGNORED="); f_info+=gcnew System::String(DoubleToString(RobotServer.cIGNORED)); f_info+=gcnew System::String(L"\r\n");
//---
textBox_Info->Text = f_info;
//---
Gdiplus::SolidBrush f_brush(Gdiplus::Color(255, 200, 200, 200)); // Кисть для заливки серым цветом.
BitmapGraphics->FillRectangle(&f_brush, (int)0, 0, 532, 496);
Gdiplus::Pen f_pen_black(Gdiplus::Color(255, 0, 0, 0), 2);
Gdiplus::Pen f_pen_R(Gdiplus::Color(255, 255, 0, 0), 2);
Gdiplus::Pen f_pen_G(Gdiplus::Color(255, 0, 255, 0), 2);
Gdiplus::Pen f_pen_B(Gdiplus::Color(255, 0, 0, 255), 2);
BitmapGraphics->DrawLine(&f_pen_black, (int)266, 0, 266, 496); // Линия сверху вниз (OY).
BitmapGraphics->DrawLine(&f_pen_black, (int)0, 248, 532, 248); // Линия слева направо (OX).
//---
int f_x = 266 + 266*RobotServer.dRX; // Вычисляем коор-ты робота ..
int f_y = 248 - 248*RobotServer.dRY; // .. в пикселях формы.
BitmapGraphics->DrawEllipse(&f_pen_G, 261+266*Last_X, 243-248*Last_Y, 10, 10); // Рисуем предыдущую точку
BitmapGraphics->DrawEllipse(&f_pen_B, 261+266*RobotServer.dTX, 243-248*RobotServer.dTY, 10, 10); // Рисуем текущую точку
//---
BitmapGraphics->DrawLine(&f_pen_B, (int)(266+266*Last_X), 248-248*Last_Y, 266+266*RobotServer.dTX, 248-248*RobotServer.dTY); // Рисуем линию меж пред и текущ точк.
//---
BitmapGraphics->DrawEllipse(&f_pen_black, f_x-20, f_y-20, 40, 40); // Рисуем робота
BitmapGraphics->DrawLine(&f_pen_G, f_x, f_y, f_x+266*RobotServer.dVRX, f_y-248*RobotServer.dVRY); // Рисуем вектор движения
BitmapGraphics->DrawLine(&f_pen_R, f_x, f_y, f_x+266*RobotClient.dFRX*0.5, f_y-248*RobotClient.dFRY*0.5); // Рисуем вектор тяги
//---
WindowGraphics->DrawImage(RobotBitmap, (int)0, 0, 532, 496); // Рисуем всё на экран
//---
LeaveCriticalSection(&Section);
}
// Обработчик Таймера 2.
private: System::Void timer2_Tick(System::Object^ sender, System::EventArgs^ e)
{
EnterCriticalSection(&Section);
// Если было установлено соединение с сервером.
if (RobotSocket) {
System::String^ f_string = gcnew System::String(L"{(FRX=");
f_string += gcnew System::String(DoubleToString(RobotClient.dFRX));
f_string += gcnew System::String(L")(FRY=");
f_string += gcnew System::String(DoubleToString(RobotClient.dFRY));
f_string += gcnew System::String(L")}");
//---
pin_ptr<const wchar_t> f_str = PtrToStringChars(f_string);
//---
char f_return[50];
unsigned long f_size = wcstombs(f_return, f_str, 50); // Преобразовать UNICODE в CHAR
//---
send(RobotSocket, f_return, f_size, 0); // Послать на сервер.
}
LeaveCriticalSection(&Section);
}};
}