diff --git a/.gitignore b/.gitignore index a250bbb..9f06cae 100644 --- a/.gitignore +++ b/.gitignore @@ -23,3 +23,11 @@ src/C++/MQTT/CMakeFiles src/C++/MQTT/Makefile src/C++/MQTT/CMakeCache.txt src/C++/MQTT/cmake_install.cmake +src/Python/flask/web/_pycache_ +venv +build/ +log +CMakeFiles/ +Makefile +CMakeCache.txt +cmake_install.cmake diff --git a/docs/Ishak/feedback.md b/docs/Ishak/feedback.md new file mode 100644 index 0000000..b9c6f1b --- /dev/null +++ b/docs/Ishak/feedback.md @@ -0,0 +1,2 @@ +# Feedback expert review + diff --git a/docs/Ishak/smartleerdoel.md b/docs/Ishak/smartleerdoel.md new file mode 100644 index 0000000..1a81a9c --- /dev/null +++ b/docs/Ishak/smartleerdoel.md @@ -0,0 +1,49 @@ +# Smart leerdoelen + +Na de retrospective die ik heb gedaan, heb ik besloten om de volgende smart leerdoelen te stellen: + +1 **Ik wil in de volgende blok meer gaan opletten op mijn teamgenoten om zo goed te weten waar iedereen mee bezig is.** + +**specifiek:** + +ik heb in onze groep de taak als voorzitter alleen merk ik op dit moment dat ik niet goed weet waar iedereen mee bezig is. Dat is niet handig omdat ik dan als voorzitter niet goed kan inschatten of iedereen op schema ligt. + +**meetbaar:** + +Ik wil dat ik aan het einde van de blok meer contact heb gehad met mijn teamgenoten en dat ik weet waar iedereen mee bezig is. Dit zorgt ervoor dat ik beter kan inschatten of iedereen op schema ligt en of ik moet ingrijpen. + +**acceptabel:** + +Dit is acceptabel omdat ik als voorzitter de taak heb om te zorgen dat iedereen op schema ligt en dat iedereen weet wat hij/zij moet doen. Zonder deze informatie kan ik niet goed mijn taak uitvoeren waardoor de teamgenoten niet goed kunnen werken. + +**realistisch:** + +Dit is realistisch omdat ik als voorzitter de taak heb om te zorgen dat iedereen op schema ligt en dat iedereen weet wat hij/zij moet doen. + +**tijdgebonden:** + +Ik wil dit doel behalen aan het einde van de blok. Dit is een realistische tijd omdat ik dan genoeg tijd heb om dit doel te behalen. Ik kan mij later ook nog tijdens de opleiding verbeteren op dit punt. + +--- + +2 **Ik wil in de volgende blok meer gaan opletten op mijn eigen documentatie.** + +**specifiek:** + +In tegen stelling tot vorig jaar hou ik nu mijn documentatie niet goed bij. Dit is niet handig omdat ik dan niet goed kan zien wat ik daadwerkelijk heb gedaan en wat ik allemaal heb geleerd. Ook is het fijn dat ik dan kan terug kijken op mijn werk als ik het later nodig heb. Ik wil daarom minimaal 2 keer per week mijn documentatie bijwerken. + +**meetbaar:** + +Ik wil dat ik aan het einde van de blok mijn documentatie goed heb bijgehouden. Dit zorgt ervoor dat ik kan zien wat ik heb gedaan en wat ik allemaal heb geleerd. Ook kan ik dan terug kijken op mijn werk als ik het later nodig heb. + +**acceptabel:** + +Dit is acceptabel omdat ik dan toekomst gericht kan werken. Ik kan dan terug kijken op mijn werk als ik het later nodig heb. Ook kan ik dan zien wat ik heb gedaan en wat ik allemaal heb geleerd. + +**realistisch:** + +Dit is realistisch omdat ik dit vorig jaar ook heb gedaan. Ik weet hoe ik mijn documentatie moet bijhouden en wat ik allemaal moet opschrijven. Mijn eis van 2 keer per week is ook realistisch omdat ik dan genoeg tijd heb om mijn documentatie bij te werken. + +**tijdgebonden:** + +Ik wil dit doel behalen aan het einde van de blok. Dit is een realistische tijd omdat ik dan genoeg tijd heb om mijn documentatie bij te werken. \ No newline at end of file diff --git a/docs/Ishak/smartleerdoel.pdf b/docs/Ishak/smartleerdoel.pdf new file mode 100644 index 0000000..88161f1 Binary files /dev/null and b/docs/Ishak/smartleerdoel.pdf differ diff --git a/src/C++/Driver/CMakeLists.txt b/src/C++/Driver/CMakeLists.txt index fbc4ec4..e6c35e2 100644 --- a/src/C++/Driver/CMakeLists.txt +++ b/src/C++/Driver/CMakeLists.txt @@ -1,12 +1,23 @@ cmake_minimum_required(VERSION 3.9) project(kobuki_control) set(CMAKE_CXX_STANDARD 23) + +# Find the Paho MQTT C++ library (static) +find_library(PAHO_MQTTPP_LIBRARY paho-mqttpp3 PATHS /usr/local/lib) +find_library(PAHO_MQTT_LIBRARY paho-mqtt3a PATHS /usr/local/lib) + +include_directories(/usr/local/include) + set(SOURCE_FILES - src/KobukiParser.cpp - src/KobukiParser.h - src/CKobuki.cpp - src/CKobuki.h - src/main.cpp) + src/KobukiDriver/KobukiParser.cpp + src/KobukiDriver/KobukiParser.h + src/KobukiDriver/CKobuki.cpp + src/KobukiDriver/CKobuki.h + src/MQTT/MqttClient.cpp + src/MQTT/MqttClient.h + src/main.cpp) add_executable(kobuki_control ${SOURCE_FILES}) -#target_link_libraries(kobuki_control ) + +# Link the static libraries +target_link_libraries(kobuki_control ${PAHO_MQTTPP_LIBRARY} ${PAHO_MQTT_LIBRARY} pthread) \ No newline at end of file diff --git a/src/C++/Driver/src/CKobuki.cpp b/src/C++/Driver/src/CKobuki.cpp deleted file mode 100755 index a667c5b..0000000 --- a/src/C++/Driver/src/CKobuki.cpp +++ /dev/null @@ -1,683 +0,0 @@ -#include "CKobuki.h" -#include "errno.h" -#include "termios.h" -#include -#include -#include - - -// plot p; -static std::vector vectorX; -static std::vector vectorY; -static std::vector vectorGyroTheta; - -// obsluha tty pod unixom -int set_interface_attribs2(int fd, int speed, int parity) -{ - struct termios tty; - memset(&tty, 0, sizeof tty); - if (tcgetattr(fd, &tty) != 0) - { - printf("error %d from tcgetattr", errno); - return -1; - } - - cfsetospeed(&tty, speed); - cfsetispeed(&tty, speed); - - tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars - // disable IGNBRK for mismatched speed tests; otherwise receive break - // as \000 chars - // tty.c_iflag &= ~IGNBRK; // disable break processing - tty.c_lflag = 0; // no signaling chars, no echo, - // no canonical processing - tty.c_oflag = 0; // no remapping, no delays - tty.c_cc[VMIN] = 0; // read doesn't block - tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout - - tty.c_iflag &= ~(IGNBRK | INLCR | ICRNL | IXON | IXOFF | - IXANY); // shut off xon/xoff ctrl - - tty.c_cflag |= (CLOCAL | CREAD); // ignore modem controls, - // enable reading - tty.c_cflag &= ~(PARENB | PARODD); // shut off parity - tty.c_cflag |= parity; - tty.c_cflag &= ~CSTOPB; - tty.c_cflag &= ~CRTSCTS; - - if (tcsetattr(fd, TCSANOW, &tty) != 0) - { - printf("error %d from tcsetattr", errno); - return -1; - } - return 0; -} - -void set_blocking2(int fd, int should_block) -{ - struct termios tty; - memset(&tty, 0, sizeof tty); - if (tcgetattr(fd, &tty) != 0) - { - printf("error %d from tggetattr", errno); - return; - } - - tty.c_cc[VMIN] = should_block ? 1 : 0; - tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout - - if (tcsetattr(fd, TCSANOW, &tty) != 0) - printf("error %d setting term attributes", errno); -} - -int CKobuki::connect(char *comportT) -{ - HCom = open(comportT, O_RDWR | O_NOCTTY | O_NONBLOCK); - - if (HCom == -1) - { - printf("Kobuki nepripojeny\n"); - return HCom; - } - else - { - set_interface_attribs2(HCom, B115200, - 0); // set speed to 115,200 bps, 8n1 (no parity) - set_blocking2(HCom, 0); // set no blocking - /* struct termios settings; - tcgetattr(HCom, &settings); - - cfsetospeed(&settings, B115200); // baud rate - settings.c_cflag &= ~PARENB; // no parity - settings.c_cflag &= ~CSTOPB; // 1 stop bit - settings.c_cflag &= ~CSIZE; - settings.c_cflag |= CS8 | CLOCAL; // 8 bits - settings.c_lflag &= ~ICANON; // canonical mode - settings.c_cc[VTIME]=1; - settings.c_oflag &= ~OPOST; // raw output - - tcsetattr(HCom, TCSANOW, &settings); // apply the settings*/ - tcflush(HCom, TCOFLUSH); - - printf("Kobuki pripojeny\n"); - return HCom; - } -} - -unsigned char *CKobuki::readKobukiMessage() -{ - unsigned char buffer[1]; - ssize_t Pocet; - buffer[0] = 0; - unsigned char *null_buffer(0); - // citame kym nezachytime zaciatok spravy - do - { - Pocet = read(HCom, buffer, 1); - } while (buffer[0] != 0xAA); - // mame zaciatok spravy (asi) - if (Pocet == 1 && buffer[0] == 0xAA) - { - // citame dalsi byte - do - { - - Pocet = read(HCom, buffer, 1); - - } while (Pocet != 1); // na linuxe -1 na windowse 0 - - // a ak je to druhy byte hlavicky - if (Pocet == 1 && buffer[0] == 0x55) - { - // precitame dlzku - Pocet = read(HCom, buffer, 1); - - // ReadFile(hCom, buffer, 1, &Pocet, NULL); - if (Pocet == 1) - { - // mame dlzku.. nastavime vektor a precitame ho cely - int readLenght = buffer[0]; - unsigned char *outputBuffer = - (unsigned char *)calloc(readLenght + 4, sizeof(char)); - outputBuffer[0] = buffer[0]; - int pct = 0; - - do - { - Pocet = 0; - int readpoc = (readLenght + 1 - pct); - Pocet = read(HCom, outputBuffer + 1 + pct, readpoc); - - pct = pct + (Pocet == -1 ? 0 : Pocet); - } while (pct != (readLenght + 1)); - - // tu si mozeme ceknut co chodi zo serial intefejsu Kobukiho - // for(int i=0;i> 8, 0x00, 0x00, 0x00}; - message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ - message[7] ^ message[8] ^ message[9] ^ message[10] ^ - message[11] ^ message[12]; - - uint32_t pocet; - pocet = write(HCom, &message, 14); -} - -void CKobuki::setRotationSpeed(double radpersec) -{ - int speedvalue = radpersec * 230.0f / 2.0f; - unsigned char message[14] = {0xaa, - 0x55, - 0x0A, - 0x0c, - 0x02, - 0xf0, - 0x00, - 0x01, - 0x04, - speedvalue % 256, - speedvalue >> 8, - 0x01, - 0x00, - 0x00}; - message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ - message[7] ^ message[8] ^ message[9] ^ message[10] ^ - message[11] ^ message[12]; - - uint32_t pocet; - pocet = write(HCom, &message, 14); -} - -void CKobuki::setArcSpeed(int mmpersec, int radius) -{ - if (radius == 0) - { - setTranslationSpeed(mmpersec); - return; - } - - int speedvalue = - mmpersec * ((radius + (radius > 0 ? 230 : -230)) / 2) / radius; - unsigned char message[14] = {0xaa, - 0x55, - 0x0A, - 0x0c, - 0x02, - 0xf0, - 0x00, - 0x01, - 0x04, - speedvalue % 256, - speedvalue >> 8, - radius % 256, - radius >> 8, - 0x00}; - message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ - message[7] ^ message[8] ^ message[9] ^ message[10] ^ - message[11] ^ message[12]; - uint32_t pocet; - pocet = write(HCom, &message, 14); -} - -void CKobuki::setSound(int noteinHz, int duration) -{ - int notevalue = floor((double)1.0 / ((double)noteinHz * 0.00000275) + 0.5); - unsigned char message[9] = {0xaa, 0x55, 0x05, - 0x03, 0x03, notevalue % 256, - notevalue >> 8, duration % 256, 0x00}; - message[8] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ - message[7]; - - uint32_t pocet; - pocet = write(HCom, &message, 9); -} - -void CKobuki::startCommunication(char *portname, bool CommandsEnabled, void *userDataL) -{ - connect(portname); - enableCommands(CommandsEnabled); - userData = userDataL; - - int pthread_result; - pthread_result = pthread_create(&threadHandle, NULL, KobukiProcess, (void *)this); - if (pthread_result != 0) { - std::cerr << "Error creating thread: " << pthread_result << std::endl; - } -} - -int CKobuki::measure() -{ - while (stopVlakno == 0) - { - unsigned char *message = readKobukiMessage(); - if (message == NULL) - { - // printf("vratil null message\n"); - continue; - } - int ok = parser.parseKobukiMessage(parser.data, message); - - // maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame - // citat - if (ok == 0) - { - loop(userData, parser.data); - } - free(message); - } - return 0; -} - -long double CKobuki::gyroToRad(signed short GyroAngle) -{ - - long double rad; - if (GyroAngle < 0) - { - rad = GyroAngle + 360; - } - else - { - rad = GyroAngle; - } - return (long double)rad * PI / 180.0; -} - -long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) -{ - if (iterationCount == 0) - { - prevLeftEncoder = Kobuki_data.EncoderLeft; - prevRightEncoder = Kobuki_data.EncoderRight; - prevTimestamp = Kobuki_data.timestamp; - prevGyroTheta = gyroToRad(Kobuki_data.GyroAngle); - iterationCount++; - } - - int dLeft; - if (abs(Kobuki_data.EncoderLeft - prevLeftEncoder) > 32000) - { - dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder + - (Kobuki_data.EncoderLeft > prevLeftEncoder ? -65536 : +65536); - } - else - { - dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder; - } - - int dRight; - if (abs(Kobuki_data.EncoderRight - prevRightEncoder) > 32000) - { - dRight = Kobuki_data.EncoderRight - prevRightEncoder + - (Kobuki_data.EncoderRight > prevRightEncoder ? -65536 : +65536); - } - else - { - dRight = Kobuki_data.EncoderRight - prevRightEncoder; - } - - long double dGyroTheta = prevGyroTheta - gyroToRad(Kobuki_data.GyroAngle); - - if (dGyroTheta > PI) - { - dGyroTheta -= 2 * PI; - } - if (dGyroTheta < -1 * PI) - { - dGyroTheta += 2 * PI; - } - - gyroTheta += dGyroTheta; - - uint16_t dTimestamp = Kobuki_data.timestamp - prevTimestamp; - - long double mLeft = dLeft * tickToMeter; - long double mRight = dRight * tickToMeter; - - if (mLeft == mRight) - { - x = x + mRight; - } - else - { - x = x + (b * (mRight + mLeft)) / (2 * (mRight - mLeft)) * - (sin((mRight - mLeft) / b + theta) - sin(theta)); - y = y + (b * (mRight + mLeft)) / (2 * (mRight - mLeft)) * - (cos((mRight - mLeft) / b + theta) - cos(theta)); - theta = (mRight - mLeft) / b + theta; - } - - displacement = (mRight + mLeft) / 2; - integratedGyroTheta = integratedGyroTheta + dGyroTheta; - gx = gx + displacement * cos(integratedGyroTheta + dGyroTheta / 2); - - gy = gy + displacement * sin(integratedGyroTheta + dGyroTheta / 2); - - totalLeft += dLeft; - totalRight += dRight; - - // ak je suma novej a predchadzajucej vacsia ako 65536 tak to pretieklo? - directionL = (prevLeftEncoder < Kobuki_data.EncoderLeft ? 1 : -1); - directionR = (prevRightEncoder < Kobuki_data.EncoderRight ? 1 : -1); - dTimestamp = (Kobuki_data.timestamp < prevTimestamp - ? prevTimestamp - Kobuki_data.timestamp + 65536 - : dTimestamp); - - prevLeftEncoder = Kobuki_data.EncoderLeft; - prevRightEncoder = Kobuki_data.EncoderRight; - prevTimestamp = Kobuki_data.timestamp; - prevGyroTheta = gyroToRad(Kobuki_data.GyroAngle); - - // std::cout << "X: " << x - // << " Y: " << y - // << " Theta: " << theta - // << "Gyro theta:" << gyroTheta - // << std::endl; - - static long counter = 0; - - //vector for data plotting - vectorX.push_back(gx); - vectorY.push_back(gy); - vectorGyroTheta.push_back(gyroTheta); - - // if (counter % 100 == 0) { - // p.plot_data(vectorY, vectorX); - // } - counter++; - - return 0; -} - - - - -// tells the kobuki to go a few meters forward or backward, the sign decides -// the function compensates for walking straight with the controller, internally it uses setArcSpeed and -// uses encoder data as feedback -void CKobuki::goStraight(long double distance){ - long double u_translation = 0; // controlled magnitude, speed of the robot in motion - long double w_translation = distance; // requested value - - // controller parameters - long double Kp_translation = 4500; - long double e_translation = 0; - int upper_thresh_translation = 600; - int lower_thresh_translation = 40; - int translation_start_gain = 20; - - long double u_rotation = 0; // controlled magnitude - long double w_rotation = 0; - long double Kp_rotation = 57; - long double e_rotation = 0; - - x = 0; - y = 0; - theta = 0; - - long i = 5; - //send command and hold until robot reaches point - while (fabs(x - w_translation) > 0.005 && x < w_translation) - { - e_translation = w_translation - x; - u_translation = Kp_translation * e_translation; - - e_rotation = w_rotation - theta; - if (!e_rotation == 0) - u_rotation = Kp_rotation / e_rotation; - - // limit translation speed - if (u_translation > upper_thresh_translation) - u_translation = upper_thresh_translation; - if (u_translation < lower_thresh_translation) - u_translation = lower_thresh_translation; - - // rewrite starting speed with line - if (i < u_translation) - { - u_translation = i; - } - - if (fabs(u_rotation) > 32767) - { - u_rotation = -32767; - } - - if (u_rotation == 0) - { - u_rotation = -32767; - } - //send command to robot - this->setArcSpeed(u_translation, u_rotation); - - // increment starting speed - i = i + translation_start_gain; - } - this->setTranslationSpeed(0); -} - -/// the method performs the rotation, it rotates using the regulator, the -/// gyroscope serves as feedback, because it is much more accurate than encoders -void CKobuki::doRotation(long double th) -{ - long double u = 0; // controlled variable, angular speed of the robot during movement - long double w = th; // desired value in radians - long double Kp = PI; - long double e = 0; - int thresh = PI / 2; - - theta = 0; - x = 0; - y = 0; - gyroTheta = 0; - - long double i = 0; - - if (w > 0) - { - while (gyroTheta < w) - { - e = w - gyroTheta; - u = Kp * e; - - if (u > thresh) - u = thresh; - if (u < 0.4) - u = 0.4; - - if (i < u) - { - u = i; - } - - std::cout << "Angle: " << gyroTheta << " required:" << w << std::endl; - this->setRotationSpeed(-1 * u); - usleep(25 * 1000); - i = i + 0.1; - } - } - else - { - while (gyroTheta > w) - { - e = w - gyroTheta; - u = Kp * e * -1; - - if (u > thresh) - u = thresh; - if (u < 0.4) - u = 0.4; - - if (i < u) - { - u = i; - } - - std::cout << "Angle: " << gyroTheta << " required:" << w << std::endl; - this->setRotationSpeed(u); - usleep(25 * 1000); - i = i + 0.1; - } - } - - std::cout << "Rotation done" << std::endl; - this->setRotationSpeed(0); - usleep(25*1000); -} - -// combines navigation to a coordinate and rotation by an angle, performs movement to -// the selected coordinate in the robot's coordinate system -void CKobuki::goToXy(long double xx, long double yy) -{ - long double th; - - yy = yy * -1; - - th = atan2(yy, xx); - doRotation(th); - - long double s = sqrt(pow(xx, 2) + pow(yy, 2)); - - // resetnem suradnicovu sustavu robota - x = 0; - y = 0; - iterationCount = 0; - theta = 0; - - // std::cout << "mam prejst: " << s << "[m]" << std::endl; - - goStraight(s); - - usleep(25 * 1000); - return; -} - -/// @brief Makes the Kobuki go forward -/// @param speedvalue speed of robot in mm/s -/// @param distance distance in meters -void CKobuki::forward(int speedvalue) { - // Use the goStraight logic to determine the speed and distance - - // Calculate the actual speed and radius values based on the conversion table - int actual_speed = speedvalue; - int actual_radius = 0; // Pure translation (straight line) - -unsigned char message[11] = { - 0xaa, // Start byte 1 - 0x55, // Start byte 2 - 0x08, // Payload length (the first 2 bytes dont count) - 0x01, // payload type (0x01 = control command) - 0x04, // Control byte or additional identifier - actual_speed % 256, // Lower byte of speed value - actual_speed >> 8, // Upper byte of speed value - 0x00, // Placeholder for radius - 0x00, // Placeholder for radius - 0x00 // Placeholder for checksum -}; - -// Calculate checksum -message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ - message[7] ^ message[8] ^ message[9]; - - // Send the message - uint32_t pocet; - pocet = write(HCom, &message, 11); - pocet = write(HCom, &message, 11); - -} - -/// @brief Makes the kobuki rotate -/// @param degrees Rotation in degrees -void CKobuki::Rotate(int degrees) { - - // convert raidans to degrees - float radians = degrees * PI / 180.0; - - // Calculate the rotation speed in radians per second - double radpersec = 1; - - // calculator rotation time and give absolute value - float rotation_time = std::abs(radians / radpersec); - - // Use original function to set the rotation speed in mm/s - setRotationSpeed(radians); - - // Sleep for the calculated rotation time - std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(rotation_time * 1000))); - - // Stop the robot after the rotation - setRotationSpeed(0); -} - - -void CKobuki::robotSafety() { - while (true) { - - if (parser.data.BumperCenter || parser.data.BumperLeft || parser.data.BumperRight || - parser.data.CliffLeft || parser.data.CliffCenter || parser.data.CliffRight) { - std::cout << "Safety condition triggered!" << std::endl; // Debug print - forward(-100); // reverse the robot - - } - - } -} - -void CKobuki::sendNullMessage(){ - - unsigned char message[11] = { - 0xaa, // Start byte 1 - 0x55, // Start byte 2 - 0x08, // Payload length (the first 2 bytes dont count) - 0x01, // payload type (0x01 = control command) - 0x04, // Control byte or additional identifier - 0x00, // Lower byte of speed value - 0x00, // Upper byte of speed value - 0x00, // Placeholder for radius - 0x00, // Placeholder for radius - 0x00 // Placeholder for checksum -}; - - -message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ - message[7] ^ message[8] ^ message[9]; - - // Send the message - uint32_t pocet; - pocet = write(HCom, &message, 11); -} diff --git a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp new file mode 100755 index 0000000..8a56f37 --- /dev/null +++ b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp @@ -0,0 +1,757 @@ +#include "CKobuki.h" +#include "errno.h" +#include "termios.h" +#include +#include + +// plot p; +static std::vector vectorX; +static std::vector vectorY; +static std::vector vectorGyroTheta; + +// obsluha tty pod unixom +int set_interface_attribs2(int fd, int speed, int parity) { + struct termios tty; + memset(&tty, 0, sizeof tty); + if (tcgetattr(fd, &tty) != 0) { + printf("error %d from tcgetattr", errno); + return -1; + } + + cfsetospeed(&tty, speed); + cfsetispeed(&tty, speed); + + tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars + // disable IGNBRK for mismatched speed tests; otherwise receive break + // as \000 chars + // tty.c_iflag &= ~IGNBRK; // disable break processing + tty.c_lflag = 0; // no signaling chars, no echo, + // no canonical processing + tty.c_oflag = 0; // no remapping, no delays + tty.c_cc[VMIN] = 0; // read doesn't block + tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout + + tty.c_iflag &= ~(IGNBRK | INLCR | ICRNL | IXON | IXOFF | + IXANY); // shut off xon/xoff ctrl + + tty.c_cflag |= (CLOCAL | CREAD); // ignore modem controls, + // enable reading + tty.c_cflag &= ~(PARENB | PARODD); // shut off parity + tty.c_cflag |= parity; + tty.c_cflag &= ~CSTOPB; + tty.c_cflag &= ~CRTSCTS; + + if (tcsetattr(fd, TCSANOW, &tty) != 0) { + printf("error %d from tcsetattr", errno); + return -1; + } + return 0; +} + +void set_blocking2(int fd, int should_block) { + struct termios tty; + memset(&tty, 0, sizeof tty); + if (tcgetattr(fd, &tty) != 0) { + printf("error %d from tggetattr", errno); + return; + } + + tty.c_cc[VMIN] = should_block ? 1 : 0; + tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout + + if (tcsetattr(fd, TCSANOW, &tty) != 0) + printf("error %d setting term attributes", errno); +} + +int CKobuki::connect(char *comportT) { + HCom = open(comportT, O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (HCom == -1) { + printf("Kobuki nepripojeny\n"); + return HCom; + } else { + set_interface_attribs2(HCom, B115200, + 0); // set speed to 115,200 bps, 8n1 (no parity) + set_blocking2(HCom, 0); // set no blocking + /* struct termios settings; + tcgetattr(HCom, &settings); + + cfsetospeed(&settings, B115200); // baud rate + settings.c_cflag &= ~PARENB; // no parity + settings.c_cflag &= ~CSTOPB; // 1 stop bit + settings.c_cflag &= ~CSIZE; + settings.c_cflag |= CS8 | CLOCAL; // 8 bits + settings.c_lflag &= ~ICANON; // canonical mode + settings.c_cc[VTIME]=1; + settings.c_oflag &= ~OPOST; // raw output + + tcsetattr(HCom, TCSANOW, &settings); // apply the settings*/ + tcflush(HCom, TCOFLUSH); + + printf("Kobuki pripojeny\n"); + return HCom; + } +} + +unsigned char *CKobuki::readKobukiMessage() { + unsigned char buffer[1]; + ssize_t Pocet; + buffer[0] = 0; + unsigned char *null_buffer(0); + // citame kym nezachytime zaciatok spravy + do { + Pocet = read(HCom, buffer, 1); + } while (buffer[0] != 0xAA); + // mame zaciatok spravy (asi) + if (Pocet == 1 && buffer[0] == 0xAA) { + // citame dalsi byte + do { + + Pocet = read(HCom, buffer, 1); + + } while (Pocet != 1); // na linuxe -1 na windowse 0 + + // a ak je to druhy byte hlavicky + if (Pocet == 1 && buffer[0] == 0x55) { + // precitame dlzku + Pocet = read(HCom, buffer, 1); + + // ReadFile(hCom, buffer, 1, &Pocet, NULL); + if (Pocet == 1) { + // mame dlzku.. nastavime vektor a precitame ho cely + int readLenght = buffer[0]; + unsigned char *outputBuffer = + (unsigned char *)calloc(readLenght + 4, sizeof(char)); + outputBuffer[0] = buffer[0]; + int pct = 0; + + do { + Pocet = 0; + int readpoc = (readLenght + 1 - pct); + Pocet = read(HCom, outputBuffer + 1 + pct, readpoc); + + pct = pct + (Pocet == -1 ? 0 : Pocet); + } while (pct != (readLenght + 1)); + + // tu si mozeme ceknut co chodi zo serial intefejsu Kobukiho + // for(int i=0;i> 8, 0x00, 0x00, 0x00}; + message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ + message[7] ^ message[8] ^ message[9] ^ message[10] ^ + message[11] ^ message[12]; + + uint32_t pocet; + pocet = write(HCom, &message, 14); +} + +void CKobuki::setRotationSpeed(double radpersec) { + int speedvalue = radpersec * 230.0f / 2.0f; + unsigned char message[14] = {0xaa, + 0x55, + 0x0A, + 0x0c, + 0x02, + 0xf0, + 0x00, + 0x01, + 0x04, + speedvalue % 256, + speedvalue >> 8, + 0x01, + 0x00, + 0x00}; + message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ + message[7] ^ message[8] ^ message[9] ^ message[10] ^ + message[11] ^ message[12]; + + uint32_t pocet; + pocet = write(HCom, &message, 14); +} + +void CKobuki::setArcSpeed(int mmpersec, int radius) { + if (radius == 0) { + setTranslationSpeed(mmpersec); + return; + } + + int speedvalue = + mmpersec * ((radius + (radius > 0 ? 230 : -230)) / 2) / radius; + unsigned char message[14] = {0xaa, + 0x55, + 0x0A, + 0x0c, + 0x02, + 0xf0, + 0x00, + 0x01, + 0x04, + speedvalue % 256, + speedvalue >> 8, + radius % 256, + radius >> 8, + 0x00}; + message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ + message[7] ^ message[8] ^ message[9] ^ message[10] ^ + message[11] ^ message[12]; + uint32_t pocet; + pocet = write(HCom, &message, 14); +} + +void CKobuki::setSound(int noteinHz, int duration) { + int notevalue = floor((double)1.0 / ((double)noteinHz * 0.00000275) + 0.5); + unsigned char message[9] = {0xaa, 0x55, 0x05, + 0x03, 0x03, notevalue % 256, + notevalue >> 8, duration % 256, 0x00}; + message[8] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ + message[7]; + + uint32_t pocet; + pocet = write(HCom, &message, 9); +} + +void CKobuki::startCommunication(char *portname, bool CommandsEnabled, + void *userDataL) { + connect(portname); + enableCommands(CommandsEnabled); + userData = userDataL; + + int pthread_result; + pthread_result = + pthread_create(&threadHandle, NULL, KobukiProcess, (void *)this); + if (pthread_result != 0) { + std::cerr << "Error creating thread: " << pthread_result << std::endl; + } +} + +int CKobuki::measure() { + while (stopVlakno == 0) { + unsigned char *message = readKobukiMessage(); + if (message == NULL) { + // printf("vratil null message\n"); + continue; + } + int ok = parser.parseKobukiMessage(parser.data, message); + + // maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame + // citat + if (ok == 0) { + loop(userData, parser.data); + } + free(message); + } + return 0; +} + +// int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) +// { +// int rtrnvalue = checkChecksum(data); +// // ak je zly checksum,tak kaslat na to +// if (rtrnvalue != 0) +// return -2; + +// int checkedValue = 1; +// // kym neprejdeme celu dlzku +// while (checkedValue < data[0]) +// { +// // basic data subload +// if (data[checkedValue] == 0x01) +// { +// checkedValue++; +// if (data[checkedValue] != 0x0F) +// return -1; +// checkedValue++; +// output.timestamp = data[checkedValue + 1] * 256 + +// data[checkedValue]; checkedValue += 2; output.BumperCenter = +// data[checkedValue] && 0x02; output.BumperLeft = +// data[checkedValue] && 0x04; output.BumperRight = +// data[checkedValue] && 0x01; checkedValue++; output.WheelDropLeft +// = data[checkedValue] && 0x02; output.WheelDropRight = +// data[checkedValue] && 0x01; checkedValue++; output.CliffCenter = +// data[checkedValue] && 0x02; output.CliffLeft = data[checkedValue] +// && 0x04; output.CliffRight = data[checkedValue] && 0x01; +// checkedValue++; +// output.EncoderLeft = data[checkedValue + 1] * 256 + +// data[checkedValue]; checkedValue += 2; output.EncoderRight = +// data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue +// += 2; output.PWMleft = data[checkedValue]; checkedValue++; +// output.PWMright = data[checkedValue]; +// checkedValue++; +// output.ButtonPress = data[checkedValue]; +// checkedValue++; +// output.Charger = data[checkedValue]; +// checkedValue++; +// output.Battery = data[checkedValue]; +// checkedValue++; +// output.overCurrent = data[checkedValue]; +// checkedValue++; +// } +// else if (data[checkedValue] == 0x03) +// { +// checkedValue++; +// if (data[checkedValue] != 0x03) +// return -3; +// checkedValue++; +// output.IRSensorRight = data[checkedValue]; +// checkedValue++; +// output.IRSensorCenter = data[checkedValue]; +// checkedValue++; +// output.IRSensorLeft = data[checkedValue]; +// checkedValue++; +// } +// else if (data[checkedValue] == 0x04) +// { +// checkedValue++; +// if (data[checkedValue] != 0x07) +// return -4; +// checkedValue++; +// output.GyroAngle = data[checkedValue + 1] * 256 + +// data[checkedValue]; checkedValue += 2; output.GyroAngleRate = +// data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue +// += 5; // 3 unsued +// } +// else if (data[checkedValue] == 0x05) +// { +// checkedValue++; +// if (data[checkedValue] != 0x06) +// return -5; +// checkedValue++; +// output.CliffSensorRight = +// data[checkedValue + 1] * 256 + data[checkedValue]; +// checkedValue += 2; +// output.CliffSensorCenter = +// data[checkedValue + 1] * 256 + data[checkedValue]; +// checkedValue += 2; +// output.CliffSensorLeft = +// data[checkedValue + 1] * 256 + data[checkedValue]; +// checkedValue += 2; +// } +// else if (data[checkedValue] == 0x06) +// { +// checkedValue++; +// if (data[checkedValue] != 0x02) +// return -6; +// checkedValue++; +// output.wheelCurrentLeft = data[checkedValue]; +// checkedValue++; +// output.wheelCurrentRight = data[checkedValue]; +// checkedValue++; +// } +// else if (data[checkedValue] == 0x0A) +// { +// checkedValue++; +// if (data[checkedValue] != 0x04) +// return -7; +// checkedValue++; +// output.extraInfo.HardwareVersionPatch = data[checkedValue]; +// checkedValue++; +// output.extraInfo.HardwareVersionMinor = data[checkedValue]; +// checkedValue++; +// output.extraInfo.HardwareVersionMajor = data[checkedValue]; +// checkedValue += 2; +// } +// else if (data[checkedValue] == 0x0B) +// { +// checkedValue++; +// if (data[checkedValue] != 0x04) +// return -8; +// checkedValue++; +// output.extraInfo.FirmwareVersionPatch = data[checkedValue]; +// checkedValue++; +// output.extraInfo.FirmwareVersionMinor = data[checkedValue]; +// checkedValue++; +// output.extraInfo.FirmwareVersionMajor = data[checkedValue]; +// checkedValue += 2; +// } +// else if (data[checkedValue] == 0x0D) +// { +// checkedValue++; +// if (data[checkedValue] % 2 != 0) +// return -9; +// checkedValue++; +// output.frameId = data[checkedValue]; +// checkedValue++; +// int howmanyFrames = data[checkedValue] / 3; +// checkedValue++; +// output.gyroData.reserve(howmanyFrames); +// output.gyroData.clear(); +// for (int hk = 0; hk < howmanyFrames; hk++) +// { +// TRawGyroData temp; +// temp.x = data[checkedValue + 1] * 256 + data[checkedValue]; +// checkedValue += 2; +// temp.y = data[checkedValue + 1] * 256 + data[checkedValue]; +// checkedValue += 2; +// temp.z = data[checkedValue + 1] * 256 + data[checkedValue]; +// checkedValue += 2; +// output.gyroData.push_back(temp); +// } +// } +// else if (data[checkedValue] == 0x10) +// { +// checkedValue++; +// if (data[checkedValue] != 0x10) +// return -10; +// checkedValue++; +// output.digitalInput = data[checkedValue + 1] * 256 + +// data[checkedValue]; checkedValue += 2; output.analogInputCh0 = +// data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue +// += 2; output.analogInputCh1 = data[checkedValue + 1] * 256 + +// data[checkedValue]; checkedValue += 2; output.analogInputCh2 = +// data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue +// += 2; output.analogInputCh3 = data[checkedValue + 1] * 256 + +// data[checkedValue]; checkedValue += 8; // 2+6 +// } +// else if (data[checkedValue] == 0x13) +// { +// checkedValue++; +// if (data[checkedValue] != 0x0C) +// return -11; +// checkedValue++; +// output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256 +// + +// data[checkedValue + 2] * 256 * 256 + +// data[checkedValue + 1] * 256 + +// data[checkedValue]; +// checkedValue += 4; +// output.extraInfo.UDID1 = data[checkedValue + 3] * 256 * 256 * 256 +// + +// data[checkedValue + 2] * 256 * 256 + +// data[checkedValue + 1] * 256 + +// data[checkedValue]; +// checkedValue += 4; +// output.extraInfo.UDID2 = data[checkedValue + 3] * 256 * 256 * 256 +// + +// data[checkedValue + 2] * 256 * 256 + +// data[checkedValue + 1] * 256 + +// data[checkedValue]; +// checkedValue += 4; +// } +// else +// { +// checkedValue++; +// checkedValue += data[checkedValue] + 1; +// } +// } +// return 0; +// } + +long double CKobuki::gyroToRad(signed short GyroAngle) { + + long double rad; + if (GyroAngle < 0) { + rad = GyroAngle + 360; + } else { + rad = GyroAngle; + } + return (long double)rad * PI / 180.0; +} + +long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) { + if (iterationCount == 0) { + prevLeftEncoder = Kobuki_data.EncoderLeft; + prevRightEncoder = Kobuki_data.EncoderRight; + prevTimestamp = Kobuki_data.timestamp; + prevGyroTheta = gyroToRad(Kobuki_data.GyroAngle); + iterationCount++; + } + + int dLeft; + if (abs(Kobuki_data.EncoderLeft - prevLeftEncoder) > 32000) { + dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder + + (Kobuki_data.EncoderLeft > prevLeftEncoder ? -65536 : +65536); + } else { + dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder; + } + + int dRight; + if (abs(Kobuki_data.EncoderRight - prevRightEncoder) > 32000) { + dRight = Kobuki_data.EncoderRight - prevRightEncoder + + (Kobuki_data.EncoderRight > prevRightEncoder ? -65536 : +65536); + } else { + dRight = Kobuki_data.EncoderRight - prevRightEncoder; + } + + long double dGyroTheta = prevGyroTheta - gyroToRad(Kobuki_data.GyroAngle); + + if (dGyroTheta > PI) { + dGyroTheta -= 2 * PI; + } + if (dGyroTheta < -1 * PI) { + dGyroTheta += 2 * PI; + } + + gyroTheta += dGyroTheta; + + uint16_t dTimestamp = Kobuki_data.timestamp - prevTimestamp; + + long double mLeft = dLeft * tickToMeter; + long double mRight = dRight * tickToMeter; + + if (mLeft == mRight) { + x = x + mRight; + } else { + x = x + (b * (mRight + mLeft)) / (2 * (mRight - mLeft)) * + (sin((mRight - mLeft) / b + theta) - sin(theta)); + y = y + (b * (mRight + mLeft)) / (2 * (mRight - mLeft)) * + (cos((mRight - mLeft) / b + theta) - cos(theta)); + theta = (mRight - mLeft) / b + theta; + } + + displacement = (mRight + mLeft) / 2; + integratedGyroTheta = integratedGyroTheta + dGyroTheta; + gx = gx + displacement * cos(integratedGyroTheta + dGyroTheta / 2); + + gy = gy + displacement * sin(integratedGyroTheta + dGyroTheta / 2); + + totalLeft += dLeft; + totalRight += dRight; + + // ak je suma novej a predchadzajucej vacsia ako 65536 tak to pretieklo? + directionL = (prevLeftEncoder < Kobuki_data.EncoderLeft ? 1 : -1); + directionR = (prevRightEncoder < Kobuki_data.EncoderRight ? 1 : -1); + dTimestamp = (Kobuki_data.timestamp < prevTimestamp + ? prevTimestamp - Kobuki_data.timestamp + 65536 + : dTimestamp); + + prevLeftEncoder = Kobuki_data.EncoderLeft; + prevRightEncoder = Kobuki_data.EncoderRight; + prevTimestamp = Kobuki_data.timestamp; + prevGyroTheta = gyroToRad(Kobuki_data.GyroAngle); + + // std::cout << "X: " << x + // << " Y: " << y + // << " Theta: " << theta + // << "Gyro theta:" << gyroTheta + // << std::endl; + + static long counter = 0; + + vectorX.push_back(gx); + vectorY.push_back(gy); + vectorGyroTheta.push_back(gyroTheta); + + // if (counter % 100 == 0) { + // p.plot_data(vectorY, vectorX); + // } + counter++; + + return 0; +} + +// tells the kobuki to go a few meters forward or backward, the sign decides +// the function compensates for walking straight with the controller, internally +// it uses setArcSpeed and uses encoder data as feedback +void CKobuki::goStraight(long double distance) { + long double u_translation = + 0; // controlled magnitude, speed of the robot in motion + long double w_translation = distance; // requested value + + // controller parameters + long double Kp_translation = 4500; + long double e_translation = 0; + int upper_thresh_translation = 600; + int lower_thresh_translation = 40; + int translation_start_gain = 20; + + long double u_rotation = 0; // controlled magnitude + long double w_rotation = 0; + long double Kp_rotation = 57; + long double e_rotation = 0; + + x = 0; + y = 0; + theta = 0; + + long i = 5; + // send command and hold until robot reaches point + while (fabs(x - w_translation) > 0.005 && x < w_translation) { + e_translation = w_translation - x; + u_translation = Kp_translation * e_translation; + + e_rotation = w_rotation - theta; + if (!e_rotation == 0) + u_rotation = Kp_rotation / e_rotation; + + // limit translation speed + if (u_translation > upper_thresh_translation) + u_translation = upper_thresh_translation; + if (u_translation < lower_thresh_translation) + u_translation = lower_thresh_translation; + + // rewrite starting speed with line + if (i < u_translation) { + u_translation = i; + } + + if (fabs(u_rotation) > 32767) { + u_rotation = -32767; + } + + if (u_rotation == 0) { + u_rotation = -32767; + } + // send command to robot + this->setArcSpeed(u_translation, u_rotation); + + // increment starting speed + i = i + translation_start_gain; + } + this->setTranslationSpeed(0); +} + +/// the method performs the rotation, it rotates using the regulator, the +/// gyroscope serves as feedback, because it is much more accurate than encoders +void CKobuki::doRotation(long double th) { + long double u = + 0; // controlled variable, angular speed of the robot during movement + long double w = th; // desired value in radians + long double Kp = PI; + long double e = 0; + long double thresh = PI / 2; + + theta = 0; + x = 0; + y = 0; + gyroTheta = 0; + + long double i = 0; + + if (w > 0) { + while (gyroTheta < w) { + e = w - gyroTheta; + u = Kp * e; + + if (u > thresh) { + u = thresh; + } + if (u < 0.4) { + u = 0.4; + } + if (i < u) { + u = i; + } + + std::cout << "Angle: " << gyroTheta << " required:" << w << std::endl; + this->setRotationSpeed(-1 * u); + usleep(25 * 1000); + i = i + 0.1; + } + } else { + while (gyroTheta > w) { + e = w - gyroTheta; + u = Kp * e * -1; + + if (u > thresh) + u = thresh; + if (u < 0.4) + u = 0.4; + + if (i < u) { + u = i; + } + + std::cout << "Angle: " << gyroTheta << " required:" << w << std::endl; + this->setRotationSpeed(u); + usleep(25 * 1000); + i = i + 0.1; + } + } + + std::cout << "Rotation done" << std::endl; + this->setRotationSpeed(0); + usleep(25 * 1000); +} + +// combines navigation to a coordinate and rotation by an angle, performs +// movement to the selected coordinate in the robot's coordinate system +void CKobuki::goToXy(long double xx, long double yy) { + long double th; + + yy = yy * -1; + + th = atan2(yy, xx); + doRotation(th); + + long double s = sqrt(pow(xx, 2) + pow(yy, 2)); + + // resetnem suradnicovu sustavu robota + x = 0; + y = 0; + iterationCount = 0; + theta = 0; + + // std::cout << "mam prejst: " << s << "[m]" << std::endl; + + goStraight(s); + + usleep(25 * 1000); + return; +} + +void CKobuki::forward(int speedvalue, long double distance) { + // Use the goStraight logic to determine the speed and distance + + // Calculate the actual speed and radius values based on the conversion table + int actual_speed = speedvalue; + int actual_radius = 0; // Pure translation (straight line) + + unsigned char message[11] = { + 0xaa, // Start byte 1 + 0x55, // Start byte 2 + 0x08, // Payload length (the first 2 bytes dont count) + 0x01, // payload type (0x01 = control command) + 0x04, // Control byte or additional identifier + actual_speed % 256, // Lower byte of speed value + actual_speed >> 8, // Upper byte of speed value + 0x00, // Placeholder for radius + 0x00, // Placeholder for radius + 0x00 // Placeholder for checksum + }; + + // Calculate checksum + message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ + message[7] ^ message[8] ^ message[9]; + + // Send the message + uint32_t pocet; + pocet = write(HCom, &message, 11); +} diff --git a/src/C++/Driver/src/CKobuki.h b/src/C++/Driver/src/KobukiDriver/CKobuki.h similarity index 100% rename from src/C++/Driver/src/CKobuki.h rename to src/C++/Driver/src/KobukiDriver/CKobuki.h diff --git a/src/C++/Driver/src/KobukiParser.cpp b/src/C++/Driver/src/KobukiDriver/KobukiParser.cpp similarity index 99% rename from src/C++/Driver/src/KobukiParser.cpp rename to src/C++/Driver/src/KobukiDriver/KobukiParser.cpp index c82ce02..df3d32d 100644 --- a/src/C++/Driver/src/KobukiParser.cpp +++ b/src/C++/Driver/src/KobukiDriver/KobukiParser.cpp @@ -4,7 +4,7 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) { int rtrnvalue = checkChecksum(data); if (rtrnvalue != 0) { - std::cerr << "Invalid checksum" << std::endl; + // std::cerr << "Invalid checksum" << std::endl; return -2; } diff --git a/src/C++/Driver/src/KobukiParser.h b/src/C++/Driver/src/KobukiDriver/KobukiParser.h similarity index 100% rename from src/C++/Driver/src/KobukiParser.h rename to src/C++/Driver/src/KobukiDriver/KobukiParser.h diff --git a/src/C++/Driver/src/graph.h b/src/C++/Driver/src/KobukiDriver/graph.h similarity index 100% rename from src/C++/Driver/src/graph.h rename to src/C++/Driver/src/KobukiDriver/graph.h diff --git a/src/C++/Driver/src/MQTT/CMakeLists.txt b/src/C++/Driver/src/MQTT/CMakeLists.txt new file mode 100644 index 0000000..ef88b41 --- /dev/null +++ b/src/C++/Driver/src/MQTT/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.10) +set(CMAKE_CXX_STANDARD 23) + +# Project name +project(mqtt_receiver) + +# Find the Paho MQTT C++ library +find_library(PAHO_MQTTPP_LIBRARY paho-mqttpp3 PATHS /usr/local/lib) +find_library(PAHO_MQTT_LIBRARY paho-mqtt3a PATHS /usr/local/lib) + +# Include the headers +include_directories(/usr/local/include) + +# Set source files +set(SOURCE_FILES + main.cpp + MqttClient.cpp + MqttClient.h +) + +# Add the executable +add_executable(mqtt_receiver ${SOURCE_FILES}) + +# Link the libraries + +# Include directories for headers +target_include_directories(mqtt_receiver PRIVATE) + +find_package(Threads REQUIRED) +target_link_libraries(mqtt_receiver Threads::Threads) diff --git a/src/C++/Driver/src/MQTT/MqttClient.cpp b/src/C++/Driver/src/MQTT/MqttClient.cpp new file mode 100644 index 0000000..7923822 --- /dev/null +++ b/src/C++/Driver/src/MQTT/MqttClient.cpp @@ -0,0 +1,61 @@ +#include "MqttClient.h" + +MqttClient::MqttClient(const std::string& address, const std::string& clientId, const std::string& username, const std::string& password) + : client_(address, clientId), username_(username), password_(password), callback_(*this) { + client_.set_callback(callback_); + connOpts_.set_clean_session(true); + connOpts_.set_mqtt_version(MQTTVERSION_3_1_1); // For MQTT 3.1.1 + if (!username_.empty() && !password_.empty()) { + connOpts_.set_user_name(username_); + connOpts_.set_password(password_); + } +} + +void MqttClient::connect() { + try { + std::cout << "Connecting to broker..." << std::endl; + client_.connect(connOpts_)->wait(); + std::cout << "Connected!" << std::endl; + } catch (const mqtt::exception& exc) { + std::cerr << "Error: " << exc.what() << std::endl; + throw; + } +} + +void MqttClient::subscribe(const std::string& topic, int qos) { + try { + std::cout << "Subscribing to topic: " << topic << std::endl; + client_.subscribe(topic, qos)->wait(); + } catch (const mqtt::exception& exc) { + std::cerr << "Error: " << exc.what() << std::endl; + throw; + } +} + +/// @brief Only needed when program doesnt keep itself alive +void MqttClient::run() { + // Keep the client running to receive messages + while (true) { + std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait to reduce CPU usage + } +} + +void MqttClient::Callback::message_arrived(mqtt::const_message_ptr msg) { + std::lock_guard lock(client_.messageMutex_); + client_.lastMessage_ = msg->to_string(); +} + +void MqttClient::Callback::connection_lost(const std::string& cause) { + std::cerr << "Connection lost. Cause: " << cause << std::endl; +} + +void MqttClient::Callback::delivery_complete(mqtt::delivery_token_ptr token) { + std::cout << "Message delivered!" << std::endl; +} + +/// @brief Get the last message received from the MQTT broker +/// @return The last message received in a string +std::string MqttClient::getLastMessage() { + std::lock_guard lock(messageMutex_); + return lastMessage_; +} \ No newline at end of file diff --git a/src/C++/Driver/src/MQTT/MqttClient.h b/src/C++/Driver/src/MQTT/MqttClient.h new file mode 100644 index 0000000..0cae258 --- /dev/null +++ b/src/C++/Driver/src/MQTT/MqttClient.h @@ -0,0 +1,38 @@ +#ifndef MQTTCLIENT_H +#define MQTTCLIENT_H + +#include +#include +#include +#include + +class MqttClient { +public: + MqttClient(const std::string& address, const std::string& clientId, const std::string& username = "", const std::string& password = ""); + void connect(); + void subscribe(const std::string& topic, int qos = 1); + void run(); + std::string getLastMessage(); + +private: + class Callback : public virtual mqtt::callback { + public: + Callback(MqttClient& client) : client_(client) {} + void message_arrived(mqtt::const_message_ptr msg) override; + void connection_lost(const std::string& cause) override; + void delivery_complete(mqtt::delivery_token_ptr token) override; + + private: + MqttClient& client_; + }; + + mqtt::async_client client_; + mqtt::connect_options connOpts_; + Callback callback_; + std::string username_; + std::string password_; + std::string lastMessage_; + std::mutex messageMutex_; +}; + +#endif // MQTTCLIENT_H \ No newline at end of file diff --git a/src/C++/Driver/src/MQTT/example.cpp b/src/C++/Driver/src/MQTT/example.cpp new file mode 100644 index 0000000..b166518 --- /dev/null +++ b/src/C++/Driver/src/MQTT/example.cpp @@ -0,0 +1,10 @@ +#include "MqttClient.h" + +int main(){ + MqttClient client("mqtt://localhost:1883", "raspberry_pi_client", "ishak", "kobuki"); + client.connect(); + client.subscribe("home/commands"); + client.run(); + + return 0; +} \ No newline at end of file diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index 6979f28..eb59b29 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -1,35 +1,45 @@ -#include "CKobuki.h" #include #include #include -#include "graph.h" +#include "KobukiDriver/graph.h" +#include "MQTT/MqttClient.h" +#include "KobukiDriver/CKobuki.h" + using namespace std; CKobuki robot; int movement(); -int checkCenterCliff(); -void logToFile(); +std::string readMQTT(); +MqttClient client("mqtt://localhost:1883", "KobukiRPI", "ishak", "kobuki"); -int main() -{ - unsigned char *null_ptr(0); - robot.startCommunication("/dev/ttyUSB0", true, null_ptr); - std::thread safety([&robot]() - { robot.robotSafety(); }); // use a lambda function to call the member function - safety.detach(); +void setup(){ + unsigned char *null_ptr(0); + robot.startCommunication("/dev/ttyUSB0", true, null_ptr); + client.connect(); + client.subscribe("home/commands"); - thread movementThread(movement); - movementThread.join(); // so the program doesnt quit - return 0; } -int checkCenterCliff() +int main(){ + setup(); + while(true){ + readMQTT(); + } + client.run(); + return 0; +} + +std::string readMQTT() { - while (true) - { - std::cout << robot.parser.data.CliffSensorRight << endl; - } + std::string message = client.getLastMessage(); + if (!message.empty()) { + std::cout << "MQTT Message: " << message << std::endl; + } + + // Add a small delay to avoid busy-waiting + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + return message; } int movement() diff --git a/src/C++/Driver/src/test.cpp b/src/C++/Driver/src/test.cpp index f846e7d..16cadf1 100644 --- a/src/C++/Driver/src/test.cpp +++ b/src/C++/Driver/src/test.cpp @@ -1,67 +1,61 @@ #include "CKobuki.h" -#include -#include -#include #include "graph.h" +#include +#include +#include using namespace std; + +// Globale instantie van de CKobuki-klasse CKobuki robot; + +// Functieprototypes int command(); +int checkCenterCliff(); +int checkWheelDrop(); -const int forward = 1; -const int ROTATE = 2; +int main() { + // Start communicatie met de robot + unsigned char *null_ptr(0); + robot.startCommunication("/dev/ttyUSB0", true, null_ptr); + usleep(1 * 1000 * 1000); + + // Start een nieuwe thread voor de command-functie + thread mv(command); + usleep(30 * 1000 * 1000); + mv.join(); // Wacht tot de command-thread klaar is -int main() -{ - unsigned char *null_ptr(0); - robot.startCommunication("/dev/ttyUSB0", true, null_ptr); - usleep(1 * 1000 * 1000); - thread mv(command); - usleep(30 * 1000 * 1000); - mv.join(); //only exit once thread one is done running } -// int checkCenterCliff() -// { -// while (true) -// { -// std::cout << "cliffsensordata:" << robot.parser.data.CliffSensorCenter << std::endl; -// } -// } +// Functie om commando's van de gebruiker te verwerken +int command() { + int input; -// int checkWheelDrop(){ -// while (true) -// { -// std::cout << "wheeldropdata:" << robot.parser.data.WheelDropLeft << std::endl; -// } -// } + while (true) { + // Vraag de gebruiker om een commando + std::cout << "choose between forward = 1 or rotate = 2" << endl; + std::cout << "What must the robot do?"; + std::cin >> input; -int command(){ - cout << "choose between forward and rotate" << endl; - cout << "What must the robot do?"; - cin >> input; + // Verwerk de invoer van de gebruiker + switch (input) { + case 1: { + int distance; + std::cout << "Enter distance to move forward: "; + std::cin >> distance; + robot.goStraight(distance); // Beweeg de robot naar voren + } break; - switch(input){ - case forward:{ - int distance; - std::cout >> "Enter distance to move forward: "; - std::cin >> distance; - robot.goStraight(distance); - } - - case ROTATE:{ - int angle; - std::cout >> "Enter angle to rotate: "; - std::cin >> angle; - robot.doRotation(angle); - } - robot.goStraight(-1); - break; - - default: - cout << "Invalid input" << endl; - break; - } - -} + case 2: { + int angle; + std::cout << "Enter angle to rotate in degrees: "; + std::cin >> angle; + robot.doRotation(angle); // Draai de robot + } break; + default: + cout << "Invalid input" << endl; // Ongeldige invoer + break; + } + } +} \ No newline at end of file diff --git a/src/C++/MQTT/CMakeLists.txt b/src/C++/MQTT/CMakeLists.txt new file mode 100644 index 0000000..1f31293 --- /dev/null +++ b/src/C++/MQTT/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.10) +set(CMAKE_CXX_STANDARD 23) + +# Find the Paho MQTT C++ library +find_library(PAHO_MQTTPP_LIBRARY paho-mqttpp3 PATHS /usr/local/lib) +find_library(PAHO_MQTT_LIBRARY paho-mqtt3a PATHS /usr/local/lib) + +# Include the headers +include_directories(/usr/local/include) + +# Add the executable +add_executable(my_program main.cpp) + +# Link the libraries +target_link_libraries(my_program ${PAHO_MQTTPP_LIBRARY} ${PAHO_MQTT_LIBRARY}) diff --git a/src/C++/MQTT/main.cpp b/src/C++/MQTT/main.cpp new file mode 100644 index 0000000..9a6529e --- /dev/null +++ b/src/C++/MQTT/main.cpp @@ -0,0 +1,56 @@ +#include +#include +#include // Voor std::this_thread::sleep_for +#include // Voor std::chrono::seconds + +const std::string ADDRESS("mqtt://localhost:1883"); // Aanpassen indien nodig +const std::string CLIENT_ID("raspberry_pi_client"); +const std::string TOPIC("home/commands"); + +class callback : public virtual mqtt::callback { + void message_arrived(mqtt::const_message_ptr msg) override { + std::cout << "Ontvangen bericht: '" << msg->get_topic() + << "' : " << msg->to_string() << std::endl; + // Doe iets met het bericht, bijvoorbeeld een GP.IO-activering + } + + void connection_lost(const std::string& cause) override { + std::cerr << "Verbinding verloren. Oorzaak: " << cause << std::endl; + } + + void delivery_complete(mqtt::delivery_token_ptr token) override { + std::cout << "Bericht afgeleverd!" << std::endl; + } +}; + +int main() { + mqtt::async_client client(ADDRESS, CLIENT_ID); + callback cb; + client.set_callback(cb); + + mqtt::connect_options connOpts; + connOpts.set_clean_session(true); + connOpts.set_user_name("ishak"); + connOpts.set_password("kobuki"); + connOpts.set_mqtt_version(MQTTVERSION_3_1_1); // Voor MQTT 3.1.1 + + try { + std::cout << "Verbinden met broker..." << std::endl; + client.connect(connOpts)->wait(); + std::cout << "Verbonden!" << std::endl; + + std::cout << "Abonneren op topic: " << TOPIC << std::endl; + client.subscribe(TOPIC, 1)->wait(); + + // Houd de client draaiende om berichten te blijven ontvangen + while (true) { + std::this_thread::sleep_for(std::chrono::seconds(1)); // Wacht om CPU-gebruik te verminderen + } + + } catch (const mqtt::exception &exc) { + std::cerr << "Fout: " << exc.what() << std::endl; + return 1; + } + + return 0; +} diff --git a/src/C++/Socket/a.out b/src/C++/Socket/a.out deleted file mode 100755 index 7b51d7a..0000000 Binary files a/src/C++/Socket/a.out and /dev/null differ diff --git a/src/C++/Socket/main.cpp b/src/C++/Socket/main.cpp deleted file mode 100644 index 1449e3a..0000000 --- a/src/C++/Socket/main.cpp +++ /dev/null @@ -1,53 +0,0 @@ -#include -#include -#include - -using boost::asio::ip::tcp; - -int main() { - try { - // Create an io_context object - boost::asio::io_context io_context; - - // Resolve the server address and port - tcp::resolver resolver(io_context); - tcp::resolver::results_type endpoints = resolver.resolve("127.0.0.1", "4024"); - - // Create and connect the socket - tcp::socket socket(io_context); - boost::asio::connect(socket, endpoints); - - std::cout << "Connected to the server." << std::endl; - - // Receive initial message from the server - boost::asio::streambuf buffer; - boost::asio::read_until(socket, buffer, "\n"); - std::istream is(&buffer); - std::string initial_message; - std::getline(is, initial_message); - std::cout << "Initial message from server: " << initial_message << std::endl; - - // Send and receive messages - while (true) { - // Send a message to the server - std::string message; - std::cout << "Enter message: "; - std::getline(std::cin, message); - message += "\n"; // Add newline to mark the end of the message - boost::asio::write(socket, boost::asio::buffer(message)); - - // Receive a response from the server - boost::asio::streambuf response_buffer; - boost::asio::read_until(socket, response_buffer, "\n"); - std::istream response_stream(&response_buffer); - std::string reply; - std::getline(response_stream, reply); - std::cout << "Reply from server: " << reply << std::endl; - } - - } catch (std::exception& e) { - std::cerr << "Exception: " << e.what() << std::endl; - } - - return 0; -} \ No newline at end of file diff --git a/src/Python/flask/web/__pycache__/app.cpython-311.pyc b/src/Python/flask/web/__pycache__/app.cpython-311.pyc new file mode 100644 index 0000000..47f94fb Binary files /dev/null and b/src/Python/flask/web/__pycache__/app.cpython-311.pyc differ diff --git a/src/Python/flask/web/app.py b/src/Python/flask/web/app.py index 5b555b0..f78aed4 100644 --- a/src/Python/flask/web/app.py +++ b/src/Python/flask/web/app.py @@ -1,17 +1,32 @@ -from flask import Flask, render_template +from flask import Flask, request, render_template, jsonify +import paho.mqtt.client as mqtt app = Flask(__name__) -@app.route('/') +# Create an MQTT client instance +mqtt_client = mqtt.Client() +mqtt_client.username_pw_set("ishak", "kobuki") +mqtt_client.connect("localhost", 1883, 60) +mqtt_client.loop_start() + +@app.route('/', methods=["POST"]) def index(): return render_template('index.html') -@app.route('/control', methods=['POST']) -def control(): - return("hello") - +@app.route('/move', methods=['POST']) +def move(): + # Get the direction from the form data + direction = request.form['direction'] + + # Publish the direction to the MQTT topic "home/commands" + result = mqtt_client.publish("home/commands", direction) + + # Check if the publish was successful + if result.rc == mqtt.MQTT_ERR_SUCCESS: + return jsonify({"message": "Bericht succesvol gepubliceerd"}), 200 + else: + return jsonify({"message": "Fout bij het publiceren van bericht"}), 500 +# Run the Flask application in debug mode if __name__ == '__main__': - app.run(debug=True) - - + app.run(debug=True) \ No newline at end of file diff --git a/src/Python/flask/web/static/script.js b/src/Python/flask/web/static/script.js new file mode 100644 index 0000000..44a0489 --- /dev/null +++ b/src/Python/flask/web/static/script.js @@ -0,0 +1,14 @@ +window.onload = function () { + const form = document.getElementById("form"); + form.onclick = function (event) { + event.preventDefault(); + fetch("/move", { + method: "GET", + }) + .then((response) => {}) + .then(() => { + fetch + }); + }; +}; +9 \ No newline at end of file diff --git a/src/Python/flask/web/static/style.css b/src/Python/flask/web/static/style.css index aa40415..1c989b5 100644 --- a/src/Python/flask/web/static/style.css +++ b/src/Python/flask/web/static/style.css @@ -10,7 +10,7 @@ body { .navbar { display: flex; justify-content: space-between; - max-width: 70rem; + max-width: 80%; background-color: #fff; border: 1px solid #f0f0f0; border-radius: 50px; @@ -56,8 +56,6 @@ body { position: relative; width: 150px; height: 150px; - margin-left: auto; - margin-right: auto; } .btn { diff --git a/src/Python/flask/web/templates/index.html b/src/Python/flask/web/templates/index.html index b824f80..705c2ed 100644 --- a/src/Python/flask/web/templates/index.html +++ b/src/Python/flask/web/templates/index.html @@ -1,19 +1,30 @@ -{%extends 'base.html'%} - -{%block head%} +{%extends 'base.html'%} {%block head%} {%endblock%} {%block content%} + + + + + + Kobuki + + +
+
+ Kobuki Robot +
+
+
+ + + + +
+
+
+
+

Sensor Data

+
+ + + {%endblock%} - -{%block content%} -
-
- Kobuki Robot -
-
- - - - -
-
-{%endblock%} \ No newline at end of file diff --git a/src/Python/socket/socketServer.py b/src/Python/socket/socketServer.py deleted file mode 100644 index 1bff9ef..0000000 --- a/src/Python/socket/socketServer.py +++ /dev/null @@ -1,20 +0,0 @@ -import socket - -HOST = "127.0.0.1" # Listen on all available interfaces -PORT = 4024 # Port to listen on (non-privileged ports are > 1023) - -with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: - s.bind((HOST, PORT)) - s.listen() - print(f"Server listening on {HOST}:{PORT}") - conn, addr = s.accept() - with conn: - print(f"Connected by {addr}") - conn.sendall(b"hallo\n") - while True: - data = conn.recv(2048) - if data: - print("Received:", repr(data)) - conn.sendall(b"message received\n") - if not data: - break \ No newline at end of file