yasarba71520/Line-Following-Robot
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge branch 'master' of https://git.efi.th-nuernberg.de/gitea/yasarba71520/Line-Following-Robot

Browse Source
This commit is contained in:
Tim Zeuner 2023-02-07 21:26:06 +01:00
commit 6577c39a1b
4 changed files with 61 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Communication/uart_communication.cpp
View File

@ -14,7 +14,7 @@ void LFR_UART::openFile(const char *fileName) {
}
int LFR_UART::writeDataToFile(uint8_t *buff, uint32_t bufferLength) {
std::cout << "Sending uart telegram" << std::endl;
//std::cout << "Sending uart telegram" << std::endl;
return write(this->fileDescriptor, buff, bufferLength);
}

63
Socket/lfr_socket.cpp
View File

@ -2,6 +2,7 @@
LFR_Socket::LFR_Socket(ExceptionCallback cb): cb(cb)
{
bufferIterator = std::begin(buffer);
if ((server_fd = socket(AF_INET, SOCK_STREAM, 0)) == -1)
{
throw std::runtime_error("Failed opening the socket");
@ -66,7 +67,7 @@ void LFR_Socket::createThread()
bool connected = false;
while(true)
{
LFR_Socket::LFR_Telegram telegram;
LFR_Socket::LFR_Telegram telegram = {};
bool received = false;
if(!stop && !listeners.empty())
{
@ -91,32 +92,46 @@ void LFR_Socket::createThread()
}
else
{
//std::cout << "accepted connection" << std::endl;
std::cout << "accepted connection" << std::endl;
pollfds[1].fd = new_socket;
connected = true;
}
}
else if (connected && pollfds[0].revents & POLLIN)
{
//std::cout << "second connection incoming" << std::endl;
std::cout << "second connection incoming" << std::endl;
int tmp_socket = accept(server_fd, (struct sockaddr *)&address, (socklen_t *)&addrlen);
close(tmp_socket);
//std::cout << "second connection closed" << std::endl;
std::cout << "second connection closed" << std::endl;
}
else if (pollfds[1].revents & POLLIN)
{
bytes_received = recv(new_socket, buffer, sizeof(buffer), 0);
if (bytes_received > 0)
LFR_Telegram tmpBuffer = {0};
bytesReceived = recv(new_socket, tmpBuffer, sizeof(tmpBuffer), 0);
if (bytesReceived > 0)
{
received = true;
std::copy(std::begin(buffer), std::end(buffer), std::begin(telegram));
//std::cout << "message received" << std::endl;
if(bufferIterator - std::begin(buffer) + bytesReceived > 1024)
{
//Too long for buffer. Clear it.
std::fill(std::begin(buffer), std::end(buffer), 0);
bufferIterator = std::begin(buffer);
}
bufferIterator = std::copy_if(
std::begin(tmpBuffer),
std::begin(tmpBuffer)+bytesReceived,
bufferIterator,
[](char c){
return(c != '\n' && c != '\r');
});
//std::cout << "bytes received: " << bytesReceived << std::endl;
//received = true;
}
else if(bytes_received == 0)
else if(bytesReceived == 0)
{
connected = false;
close(pollfds[1].fd);
//std::cout << "connection closed by peer" << std::endl;
std::cout << "connection closed by peer" << std::endl;
}
}
}
@ -131,6 +146,32 @@ void LFR_Socket::createThread()
}
}
// Search the buffer for the start of a telegram
char *telegramStart = NULL;
telegramStart = strstr(buffer, "aa");
if(telegramStart && telegramStart != std::begin(buffer)) {
//Move the content of the buffer
auto delta = bufferIterator - telegramStart;
std::copy(telegramStart, std::end(buffer), std::begin(buffer));
bufferIterator = std::begin(buffer) + delta;
}
// Search the buffer for the end of a telegram
char *telegramEnd = NULL;
telegramEnd = strstr(buffer, "zz");
if(telegramEnd && telegramStart && telegramEnd-telegramStart>2) {
std::copy(telegramStart+2, telegramEnd, telegram);
received = true;
std::copy(telegramEnd+2, std::end(buffer), std::begin(buffer));
bufferIterator = std::begin(buffer);
}
else if(telegramEnd && telegramStart && telegramEnd-telegramStart<=2)
{
//Got an empty telegram
std::copy(telegramEnd, std::end(buffer), std::begin(buffer));
bufferIterator = std::begin(buffer);
}
//Invoke the callback method (ListenerPair second -> ListenerCallback)
if (received)
{

4
Socket/lfr_socket.h
View File

@ -13,6 +13,7 @@
#include <string>
#include <algorithm>
#include <exception>
#include <cstring>
class LFR_Socket
{
@ -31,11 +32,12 @@ private:
std::unique_ptr<std::thread> thread;
mutable std::mutex mutex;
int server_fd, new_socket, bytes_received;
int server_fd, new_socket, bytesReceived;
struct sockaddr_in address;
int opt = 1;
int addrlen = sizeof(address);
char buffer[1024] = {0};
char* bufferIterator;
//void provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi);

11
lfr_state_machine.cpp
View File

@ -57,9 +57,13 @@ void LFR_StateMachine::parseString(string s)
std::vector<std::string> splitStr = split(s, ";");
if(!checkStringValidity(splitStr))
{
std::cout<<"Invalid String" << std::endl;
std::cout<<"Error: Invalid String: " << s << std::endl;
return;
}
else
{
std::cout<< s << std::endl;
}
double wheels[4] = {0.0, 0.0, 0.0, 0.0};
int mode = std::stoi(splitStr[0]);
if(mode == 0) {
@ -121,11 +125,6 @@ void LFR_StateMachine::setState(LFR_IState& newState)
currentState = &newState;
currentState->enter(this);
}
else
{
std::cout << "no switch" << std::endl;
}
}
void LFR_StateMachine::enterAutonomous()