From b9c5872a10a77332e41adfa05b7222347cf1cc52 Mon Sep 17 00:00:00 2001
From: Filippo Brizzi <brizzi@filics.eu>
Date: Thu, 21 Aug 2025 17:02:34 +0200
Subject: [PATCH 1/2] fix muiltiplex

---
 lib/cpp/examples/simple.cc               |  4 ++--
 lib/cpp/mjbots/moteus/moteus_multiplex.h | 25 +++++++++++-------------
 2 files changed, 13 insertions(+), 16 deletions(-)

diff --git a/lib/cpp/examples/simple.cc b/lib/cpp/examples/simple.cc
index 6d9e18666..fe6285cfc 100644
--- a/lib/cpp/examples/simple.cc
+++ b/lib/cpp/examples/simple.cc
@@ -48,8 +48,8 @@ int main(int argc, char** argv) {
 
     // Here we will just command a position of NaN and a velocity of
     // 0.0.  This means "hold position wherever you are".
-    cmd.position = std::numeric_limits<double>::quiet_NaN();
-    cmd.velocity = 0.0;
+    cmd.position = 100; //std::numeric_limits<double>::quiet_NaN();
+    cmd.velocity = 1.0;
 
     const auto maybe_result = controller.SetPosition(cmd);
     if (maybe_result) {
diff --git a/lib/cpp/mjbots/moteus/moteus_multiplex.h b/lib/cpp/mjbots/moteus/moteus_multiplex.h
index 8b46ad146..6bb69d840 100644
--- a/lib/cpp/mjbots/moteus/moteus_multiplex.h
+++ b/lib/cpp/mjbots/moteus/moteus_multiplex.h
@@ -14,9 +14,6 @@
 
 #pragma once
 
-#include <iomanip>
-#include <iostream>
-
 #include <math.h>
 #include <stdint.h>
 #include <string.h>
@@ -303,11 +300,12 @@ class MultiplexParser {
     uint16_t value = 0;
     Resolution resolution = kIgnore;
     int8_t command = 0;
+    int8_t count = 0;
 
     Result(bool done_in, uint16_t value_in, Resolution resolution_in,
-           int8_t command_in)
+           int8_t command_in, int8_t count_in)
         : done(done_in), value(value_in), resolution(resolution_in),
-          command(command_in) {}
+          command(command_in), count(count_in) {}
 
     Result() {}
   };
@@ -315,7 +313,7 @@ class MultiplexParser {
   Result next() {
     if (offset_ >= size_) {
       // We are done.
-      return Result(true, 0, Resolution::kInt8, Multiplex::kNop);
+      return Result(true, 0, Resolution::kInt8, Multiplex::kNop, 0);
     }
 
     if (remaining_) {
@@ -324,11 +322,11 @@ class MultiplexParser {
 
       // Do we actually have enough data?
       if (offset_ + ResolutionSize(current_resolution_) > size_) {
-        return Result(true, 0, Resolution::kInt8, Multiplex::kNop);
+        return Result(true, 0, Resolution::kInt8, Multiplex::kNop, 0);
       }
 
-      return Result(false, this_register, current_resolution_,
-                    current_command_);
+      return Result(false, this_register, current_resolution_, current_command_,
+                    remaining_);
     }
 
     // We need to look for another command.
@@ -375,24 +373,23 @@ class MultiplexParser {
           // Empty, guess we can ignore.
           continue;
         }
-
         current_register_ = ReadVaruint();
 
         // If it's a read registry we do not need to look further.
         if (current_command_ >= 0x10 && current_command_ < 0x20) {
           remaining_ = 0;
           return Result(false, current_register_++, current_resolution_,
-                        current_command_);
+                        current_command_, count);
         }
 
         remaining_ = count - 1;
 
         if (offset_ + ResolutionSize(current_resolution_) > size_) {
-          return Result(true, 0, Resolution::kInt8, Multiplex::kNop);
+          return Result(true, 0, Resolution::kInt8, Multiplex::kNop, 0);
         }
 
         return Result(false, current_register_++, current_resolution_,
-                      current_command_);
+                      current_command_, count);
       }
 
       // For anything else, we'll just assume it is an error of some
@@ -400,7 +397,7 @@ class MultiplexParser {
       offset_ = size_;
       break;
     }
-    return Result(true, 0, Resolution::kInt8, Multiplex::kNop);
+    return Result(true, 0, Resolution::kInt8, Multiplex::kNop, 0);
   }
 
   template <typename T> T Read() {

From c27a30b4171e34a02f18cf100903f13bb97cbbce Mon Sep 17 00:00:00 2001
From: Filippo Brizzi <brizzi@filics.eu>
Date: Thu, 21 Aug 2025 17:05:52 +0200
Subject: [PATCH 2/2] rever chages to example

---
 lib/cpp/examples/simple.cc | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/lib/cpp/examples/simple.cc b/lib/cpp/examples/simple.cc
index fe6285cfc..6d9e18666 100644
--- a/lib/cpp/examples/simple.cc
+++ b/lib/cpp/examples/simple.cc
@@ -48,8 +48,8 @@ int main(int argc, char** argv) {
 
     // Here we will just command a position of NaN and a velocity of
     // 0.0.  This means "hold position wherever you are".
-    cmd.position = 100; //std::numeric_limits<double>::quiet_NaN();
-    cmd.velocity = 1.0;
+    cmd.position = std::numeric_limits<double>::quiet_NaN();
+    cmd.velocity = 0.0;
 
     const auto maybe_result = controller.SetPosition(cmd);
     if (maybe_result) {