From d6dfbbd6d1bac0c7f1cd7b39e5659cb38ad375b4 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Mon, 6 Nov 2023 14:57:36 +0100 Subject: [PATCH 1/2] Remove any buffering from stout and stderr to get the printed information immediately. --- lib/ArduinoNative/Arduino.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/lib/ArduinoNative/Arduino.cpp b/lib/ArduinoNative/Arduino.cpp index ea6a8382..f692fb07 100644 --- a/lib/ArduinoNative/Arduino.cpp +++ b/lib/ArduinoNative/Arduino.cpp @@ -171,6 +171,10 @@ extern int main(int argc, char** argv) Keyboard& keyboard = Board::getInstance().getKeyboard(); PrgArguments prgArguments; + /* Remove any buffering from stout and stderr to get the printed information immediately. */ + (void)setvbuf(stdout, NULL, _IONBF, 0); + (void)setvbuf(stderr, NULL, _IONBF, 0); + status = handleCommandLineArguments(prgArguments, argc, argv); if (0 == status) @@ -303,10 +307,10 @@ static int handleCommandLineArguments(PrgArguments& prgArguments, int argc, char errno = 0; /* Reset Error Register */ long parsedValue = strtol(optarg, &p, 10); /* Long value parsed from string. */ - if (('\0' == *p) && /* Make sure the string is completely read. */ - (0 == errno) && /* No Errors were produced. */ - (UINT16_MAX >= parsedValue) && /* No overflow of uint16_t to allow direct casting. */ - (0U <= parsedValue)) /* No negative values. */ + if (('\0' == *p) && /* Make sure the string is completely read. */ + (0 == errno) && /* No Errors were produced. */ + (UINT16_MAX >= parsedValue) && /* No overflow of uint16_t to allow direct casting. */ + (0U <= parsedValue)) /* No negative values. */ { prgArguments.socketServerPort = parsedValue; } @@ -330,7 +334,7 @@ static int handleCommandLineArguments(PrgArguments& prgArguments, int argc, char case 'h': /* Help */ /* fallthrough */ - default: /* Default */ + default: /* Default */ printf("Usage: %s \nOptions:\n", programName); printf("\t-h\t\t\tShow this help message.\n"); /* Help */ printf("\t-p \tSet SocketServer port.\n"); /* Port */ From 6de173ce1ea474740bc484f7b7a672f06a3a4684 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Mon, 6 Nov 2023 15:09:14 +0100 Subject: [PATCH 2/2] Fixed orientation wrapping --- doc/architecture/README.md | 4 ++++ lib/Service/Odometry.cpp | 15 +-------------- 2 files changed, 5 insertions(+), 14 deletions(-) diff --git a/doc/architecture/README.md b/doc/architecture/README.md index 0444ab60..67b169ba 100644 --- a/doc/architecture/README.md +++ b/doc/architecture/README.md @@ -70,6 +70,10 @@ Base equations: Orientation: * $alpha [rad] = \frac{distanceRight [mm] - distanceLeft [mm]}{wheelBase [mm]}$ * $orientation' [rad] = orientation [rad] + alpha [rad]$ +* $orientation' [rad] = orientation [rad]~\%~2\pi$ +* $-2\pi < Orientation < 2\pi$ +* After wrapping on the positive limit $2\pi$, the orientation remains positive and starts from 0 again. +* After wrapping on the negative limit $2\pi$, the orientation remains negative and starts from 0 again. Position: * $dX [mm] = -distanceCenter [mm] \cdot sin(orientation' [rad])$ <- Approximation for performance reason diff --git a/lib/Service/Odometry.cpp b/lib/Service/Odometry.cpp index 3adca1ea..7f3c003f 100644 --- a/lib/Service/Odometry.cpp +++ b/lib/Service/Odometry.cpp @@ -197,20 +197,7 @@ int32_t Odometry::calculateOrientation(int32_t orientation, int16_t stepsLeft, i /* Calculate orientation */ orientation += alpha; - - /* -2*PI < alpha < +2*PI */ - if (FP_2PI() < orientation) - { - orientation = orientation - FP_4PI(); - } - else if (-FP_2PI() > orientation) - { - orientation = orientation + FP_4PI(); - } - else - { - ; - } + orientation %= FP_2PI(); /* -2*PI < orientation < +2*PI */ return orientation; }