Merge pull request #31 from BlueAndi/dev

Fixed orientation wrapping and removed buffering
BlueAndi · Nov 6, 2023 · fdc41f6 · fdc41f6
2 parents 396c3be + 6de173c
commit fdc41f6
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 19 deletions.
diff --git 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/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 <option(s)>\nOptions:\n", programName);
             printf("\t-h\t\t\tShow this help message.\n");          /* Help */
             printf("\t-p <PORT NUMBER>\tSet SocketServer port.\n"); /* Port */

diff --git 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;
 }