mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-03 03:45:00 +00:00
Merge branch 'test_script_movement'
This commit is contained in:
8
.gitignore
vendored
8
.gitignore
vendored
@@ -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
|
||||
|
||||
2
docs/Ishak/feedback.md
Normal file
2
docs/Ishak/feedback.md
Normal file
@@ -0,0 +1,2 @@
|
||||
# Feedback expert review
|
||||
|
||||
49
docs/Ishak/smartleerdoel.md
Normal file
49
docs/Ishak/smartleerdoel.md
Normal file
@@ -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.
|
||||
BIN
docs/Ishak/smartleerdoel.pdf
Normal file
BIN
docs/Ishak/smartleerdoel.pdf
Normal file
Binary file not shown.
@@ -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/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)
|
||||
@@ -1,683 +0,0 @@
|
||||
#include "CKobuki.h"
|
||||
#include "errno.h"
|
||||
#include "termios.h"
|
||||
#include <cstddef>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
|
||||
|
||||
// plot p;
|
||||
static std::vector<float> vectorX;
|
||||
static std::vector<float> vectorY;
|
||||
static std::vector<float> 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<outputBuffer[0]+2;i++)
|
||||
// {
|
||||
// printf("%x ",outputBuffer[i]);
|
||||
// }
|
||||
|
||||
return outputBuffer;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return null_buffer;
|
||||
}
|
||||
|
||||
void CKobuki::setLed(int led1, int led2)
|
||||
{
|
||||
unsigned char message[8] = {0xaa, 0x55, 0x04, 0x0c, 0x02, 0x00, (unsigned char)((led1 + led2 * 4) % 256), 0x00};
|
||||
message[7] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6];
|
||||
uint32_t pocet;
|
||||
pocet = write(HCom, &message, 8);
|
||||
}
|
||||
|
||||
// tato funkcia nema moc sama o sebe vyznam, payload o tom, ze maju byt externe
|
||||
// napajania aktivne musi byt aj tak v kazdej sprave...
|
||||
void CKobuki::setPower(int value)
|
||||
{
|
||||
if (value == 1)
|
||||
{
|
||||
unsigned char message[8] = {0xaa, 0x55, 0x04, 0x0C, 0x02, 0xf0, 0x00, 0xAF};
|
||||
uint32_t pocet;
|
||||
pocet = write(HCom, &message, 8);
|
||||
}
|
||||
}
|
||||
|
||||
void CKobuki::setTranslationSpeed(int mmpersec)
|
||||
{
|
||||
unsigned char message[14] = {0xaa, 0x55, 0x0A, 0x0c, 0x02,
|
||||
0xf0, 0x00, 0x01, 0x04, mmpersec % 256,
|
||||
mmpersec >> 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<int>(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);
|
||||
}
|
||||
757
src/C++/Driver/src/KobukiDriver/CKobuki.cpp
Executable file
757
src/C++/Driver/src/KobukiDriver/CKobuki.cpp
Executable file
@@ -0,0 +1,757 @@
|
||||
#include "CKobuki.h"
|
||||
#include "errno.h"
|
||||
#include "termios.h"
|
||||
#include <cstddef>
|
||||
#include <iostream>
|
||||
|
||||
// plot p;
|
||||
static std::vector<float> vectorX;
|
||||
static std::vector<float> vectorY;
|
||||
static std::vector<float> 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<outputBuffer[0]+2;i++)
|
||||
// {
|
||||
// printf("%x ",outputBuffer[i]);
|
||||
// }
|
||||
|
||||
return outputBuffer;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return null_buffer;
|
||||
}
|
||||
|
||||
void CKobuki::setLed(int led1, int led2) {
|
||||
unsigned char message[8] = {0xaa,
|
||||
0x55,
|
||||
0x04,
|
||||
0x0c,
|
||||
0x02,
|
||||
0x00,
|
||||
(unsigned char)((led1 + led2 * 4) % 256),
|
||||
0x00};
|
||||
message[7] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6];
|
||||
uint32_t pocet;
|
||||
pocet = write(HCom, &message, 8);
|
||||
}
|
||||
|
||||
// tato funkcia nema moc sama o sebe vyznam, payload o tom, ze maju byt externe
|
||||
// napajania aktivne musi byt aj tak v kazdej sprave...
|
||||
void CKobuki::setPower(int value) {
|
||||
if (value == 1) {
|
||||
unsigned char message[8] = {0xaa, 0x55, 0x04, 0x0C, 0x02, 0xf0, 0x00, 0xAF};
|
||||
uint32_t pocet;
|
||||
pocet = write(HCom, &message, 8);
|
||||
}
|
||||
}
|
||||
|
||||
void CKobuki::setTranslationSpeed(int mmpersec) {
|
||||
unsigned char message[14] = {0xaa, 0x55, 0x0A, 0x0c, 0x02,
|
||||
0xf0, 0x00, 0x01, 0x04, mmpersec % 256,
|
||||
mmpersec >> 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);
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
30
src/C++/Driver/src/MQTT/CMakeLists.txt
Normal file
30
src/C++/Driver/src/MQTT/CMakeLists.txt
Normal file
@@ -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)
|
||||
61
src/C++/Driver/src/MQTT/MqttClient.cpp
Normal file
61
src/C++/Driver/src/MQTT/MqttClient.cpp
Normal file
@@ -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<std::mutex> 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<std::mutex> lock(messageMutex_);
|
||||
return lastMessage_;
|
||||
}
|
||||
38
src/C++/Driver/src/MQTT/MqttClient.h
Normal file
38
src/C++/Driver/src/MQTT/MqttClient.h
Normal file
@@ -0,0 +1,38 @@
|
||||
#ifndef MQTTCLIENT_H
|
||||
#define MQTTCLIENT_H
|
||||
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
#include <mutex>
|
||||
#include <mqtt/async_client.h>
|
||||
|
||||
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
|
||||
10
src/C++/Driver/src/MQTT/example.cpp
Normal file
10
src/C++/Driver/src/MQTT/example.cpp
Normal file
@@ -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;
|
||||
}
|
||||
@@ -1,35 +1,45 @@
|
||||
#include "CKobuki.h"
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <thread>
|
||||
#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()
|
||||
{
|
||||
|
||||
void setup(){
|
||||
unsigned char *null_ptr(0);
|
||||
robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
|
||||
client.connect();
|
||||
client.subscribe("home/commands");
|
||||
|
||||
std::thread safety([&robot]()
|
||||
{ robot.robotSafety(); }); // use a lambda function to call the member function
|
||||
safety.detach();
|
||||
}
|
||||
|
||||
thread movementThread(movement);
|
||||
movementThread.join(); // so the program doesnt quit
|
||||
int main(){
|
||||
setup();
|
||||
while(true){
|
||||
readMQTT();
|
||||
}
|
||||
client.run();
|
||||
return 0;
|
||||
}
|
||||
|
||||
int checkCenterCliff()
|
||||
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()
|
||||
|
||||
@@ -1,67 +1,61 @@
|
||||
#include "CKobuki.h"
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <thread>
|
||||
#include "graph.h"
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
|
||||
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()
|
||||
{
|
||||
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(); //only exit once thread one is done running
|
||||
mv.join(); // Wacht tot de command-thread klaar is
|
||||
|
||||
}
|
||||
|
||||
// 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;
|
||||
|
||||
switch(input){
|
||||
case forward:{
|
||||
// Verwerk de invoer van de gebruiker
|
||||
switch (input) {
|
||||
case 1: {
|
||||
int distance;
|
||||
std::cout >> "Enter distance to move forward: ";
|
||||
std::cout << "Enter distance to move forward: ";
|
||||
std::cin >> distance;
|
||||
robot.goStraight(distance);
|
||||
}
|
||||
robot.goStraight(distance); // Beweeg de robot naar voren
|
||||
} break;
|
||||
|
||||
case ROTATE:{
|
||||
case 2: {
|
||||
int angle;
|
||||
std::cout >> "Enter angle to rotate: ";
|
||||
std::cout << "Enter angle to rotate in degrees: ";
|
||||
std::cin >> angle;
|
||||
robot.doRotation(angle);
|
||||
}
|
||||
robot.goStraight(-1);
|
||||
break;
|
||||
robot.doRotation(angle); // Draai de robot
|
||||
} break;
|
||||
|
||||
default:
|
||||
cout << "Invalid input" << endl;
|
||||
cout << "Invalid input" << endl; // Ongeldige invoer
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
15
src/C++/MQTT/CMakeLists.txt
Normal file
15
src/C++/MQTT/CMakeLists.txt
Normal file
@@ -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})
|
||||
56
src/C++/MQTT/main.cpp
Normal file
56
src/C++/MQTT/main.cpp
Normal file
@@ -0,0 +1,56 @@
|
||||
#include <iostream>
|
||||
#include <mqtt/async_client.h>
|
||||
#include <thread> // Voor std::this_thread::sleep_for
|
||||
#include <chrono> // 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;
|
||||
}
|
||||
Binary file not shown.
@@ -1,53 +0,0 @@
|
||||
#include <iostream>
|
||||
#include <boost/asio.hpp>
|
||||
#include <string>
|
||||
|
||||
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;
|
||||
}
|
||||
BIN
src/Python/flask/web/__pycache__/app.cpython-311.pyc
Normal file
BIN
src/Python/flask/web/__pycache__/app.cpython-311.pyc
Normal file
Binary file not shown.
@@ -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)
|
||||
|
||||
|
||||
|
||||
14
src/Python/flask/web/static/script.js
Normal file
14
src/Python/flask/web/static/script.js
Normal file
@@ -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
|
||||
@@ -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 {
|
||||
|
||||
@@ -1,19 +1,30 @@
|
||||
{%extends 'base.html'%}
|
||||
|
||||
{%block head%}
|
||||
|
||||
{%endblock%}
|
||||
|
||||
{%block content%}
|
||||
<div class="container">
|
||||
{%extends 'base.html'%} {%block head%} {%endblock%} {%block content%}
|
||||
<!DOCTYPE html>
|
||||
<html lang="en">
|
||||
<head>
|
||||
<meta charset="UTF-8" />
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0" />
|
||||
<title>Kobuki</title>
|
||||
</head>
|
||||
<body>
|
||||
<div class="container">
|
||||
<div class="image-section">
|
||||
<img src="kobuki.jpg" alt="Kobuki Robot" id="robot-image">
|
||||
<img src="kobuki.jpg" alt="Kobuki Robot" id="robot-image" />
|
||||
</div>
|
||||
<div class="button-section">
|
||||
<button class="btn">1</button>
|
||||
<button class="btn">2</button>
|
||||
<button class="btn">3</button>
|
||||
<button class="btn">4</button>
|
||||
<form id = "form" action="/" method="post">
|
||||
<button class="btn" name="direction" value="left">←</button>
|
||||
<button class="btn" name="direction" value="up">↑</button>
|
||||
<button class="btn" name="direction" value="right">→</button>
|
||||
<button class="btn" name="direction" value="down">↓</button>
|
||||
</form>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="container">
|
||||
<h1>Sensor Data</h1>
|
||||
</div>
|
||||
<script href="../static/script.js"></script>
|
||||
</body>
|
||||
</html>
|
||||
|
||||
{%endblock%}
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user