Smikkelbakje/J2S1-Kobuki
1
0
Fork 0
mirror of https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git synced 2025-08-03 20:04:58 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

commentaar naar engels

Browse Source
This commit is contained in:
ishak jmilou.ishak
2025-01-06 09:53:01 +01:00
parent b29a615681
commit bb1904b125
1 changed files with 20 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
src/C++/Driver/src/KobukiDriver/CKobuki.cpp
View File

@@ -68,7 +68,7 @@ int CKobuki::connect(char *comportT) {
HCom = open(comportT, O_RDWR | O_NOCTTY | O_NONBLOCK); HCom = open(comportT, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (HCom == -1) { if (HCom == -1) {
printf("Kobuki nepripojeny\n"); printf("Kobuki connected\n");
return HCom; return HCom;
} else { } else {
set_interface_attribs2(HCom, B115200, set_interface_attribs2(HCom, B115200,
@@ -89,7 +89,7 @@ int CKobuki::connect(char *comportT) {
tcsetattr(HCom, TCSANOW, &settings); // apply the settings*/ tcsetattr(HCom, TCSANOW, &settings); // apply the settings*/
tcflush(HCom, TCOFLUSH); tcflush(HCom, TCOFLUSH);
printf("Kobuki pripojeny\n"); printf("Kobuki connected\n");
return HCom; return HCom;
} }
} }
@@ -99,27 +99,26 @@ unsigned char *CKobuki::readKobukiMessage() {
ssize_t Pocet; ssize_t Pocet;
buffer[0] = 0; buffer[0] = 0;
unsigned char *null_buffer(0); unsigned char *null_buffer(0);
// citame kym nezachytime zaciatok spravy
// Read until the start of the message is detected
do { do {
Pocet = read(HCom, buffer, 1); Pocet = read(HCom, buffer, 1);
} while (buffer[0] != 0xAA); } while (buffer[0] != 0xAA);
// mame zaciatok spravy (asi)
// We have the start of the message (possibly)
if (Pocet == 1 && buffer[0] == 0xAA) { if (Pocet == 1 && buffer[0] == 0xAA) {
// citame dalsi byte // Read the next byte
do { do {
Pocet = read(HCom, buffer, 1); Pocet = read(HCom, buffer, 1);
} while (Pocet != 1); // On Linux: -1, on Windows: 0
} while (Pocet != 1); // na linuxe -1 na windowse 0 // If it is the second byte of the header
// a ak je to druhy byte hlavicky
if (Pocet == 1 && buffer[0] == 0x55) { if (Pocet == 1 && buffer[0] == 0x55) {
// precitame dlzku // Read the length
Pocet = read(HCom, buffer, 1); Pocet = read(HCom, buffer, 1);
// ReadFile(hCom, buffer, 1, &Pocet, NULL);
if (Pocet == 1) { if (Pocet == 1) {
// mame dlzku.. nastavime vektor a precitame ho cely // We have the length; initialize a buffer and read the entire message
int readLenght = buffer[0]; int readLenght = buffer[0];
unsigned char *outputBuffer = unsigned char *outputBuffer =
(unsigned char *)calloc(readLenght + 4, sizeof(char)); (unsigned char *)calloc(readLenght + 4, sizeof(char));
@@ -134,7 +133,7 @@ unsigned char *CKobuki::readKobukiMessage() {
pct = pct + (Pocet == -1 ? 0 : Pocet); pct = pct + (Pocet == -1 ? 0 : Pocet);
} while (pct != (readLenght + 1)); } while (pct != (readLenght + 1));
// tu si mozeme ceknut co chodi zo serial intefejsu Kobukiho // Here we can check what data is received from the Kobuki's serial interface
// for(int i=0;i<outputBuffer[0]+2;i++) // for(int i=0;i<outputBuffer[0]+2;i++)
// { // {
// printf("%x ",outputBuffer[i]); // printf("%x ",outputBuffer[i]);
@@ -162,8 +161,8 @@ void CKobuki::setLed(int led1, int led2) {
pocet = write(HCom, &message, 8); pocet = write(HCom, &message, 8);
} }
// tato funkcia nema moc sama o sebe vyznam, payload o tom, ze maju byt externe // this function doesn't have much meaning by itself, the payload about them being external
// napajania aktivne musi byt aj tak v kazdej sprave... // power supplies must be active in every message anyway...
void CKobuki::setPower(int value) { void CKobuki::setPower(int value) {
if (value == 1) { if (value == 1) {
unsigned char message[8] = {0xaa, 0x55, 0x04, 0x0C, 0x02, 0xf0, 0x00, 0xAF}; unsigned char message[8] = {0xaa, 0x55, 0x04, 0x0C, 0x02, 0xf0, 0x00, 0xAF};
@@ -267,13 +266,13 @@ int CKobuki::measure() {
while (stopVlakno == 0) { while (stopVlakno == 0) {
unsigned char *message = readKobukiMessage(); unsigned char *message = readKobukiMessage();
if (message == NULL) { if (message == NULL) {
// printf("vratil null message\n"); // printf("returned null message\n");
continue; continue;
} }
int ok = parser.parseKobukiMessage(parser.data, message); int ok = parser.parseKobukiMessage(parser.data, message);
// maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame // the maximum callback function can take 20 ms, if it takes longer, we won't be able to do it
// citat // read
if (ok == 0) { if (ok == 0) {
loop(userData, parser.data); loop(userData, parser.data);
} }
@@ -353,7 +352,7 @@ long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) {
totalLeft += dLeft; totalLeft += dLeft;
totalRight += dRight; totalRight += dRight;
// ak je suma novej a predchadzajucej vacsia ako 65536 tak to pretieklo? // if the sum of the new and previous is greater than 65536 then it overflowed?
directionL = (prevLeftEncoder < Kobuki_data.EncoderLeft ? 1 : -1); directionL = (prevLeftEncoder < Kobuki_data.EncoderLeft ? 1 : -1);
directionR = (prevRightEncoder < Kobuki_data.EncoderRight ? 1 : -1); directionR = (prevRightEncoder < Kobuki_data.EncoderRight ? 1 : -1);
dTimestamp = (Kobuki_data.timestamp < prevTimestamp dTimestamp = (Kobuki_data.timestamp < prevTimestamp
@@ -387,7 +386,7 @@ long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) {
// tells the kobuki to go a few meters forward or backward, the sign decides // tells the kobuki to go a few meters forward or backward, the sign decides
// the function compensates for walking straight with the controller, internally // the function compensates for walking straight with the controller, internally
// it uses setArcSpeed and uses encoder data as feedback // it uses setArcSpeed ​​and uses encoder data as feedback
void CKobuki::goStraight(long double distance) { void CKobuki::goStraight(long double distance) {
long double u_translation = long double u_translation =
0; // controlled magnitude, speed of the robot in motion 0; // controlled magnitude, speed of the robot in motion