Repository: project-sternbergia/cybergear_m5 (language: cpp)

Issue:
Connection to 4 or more motors
When using this library, if I try to connect to 4 motors,
I can get the information of 0, 1, and 3 motors, but I cannot get the information of the second motor.
Do you know the cause of this problem?

For example
std::vector<uint8_t> motor_ids = {0x7F, 0x7E, 0x7D, 0x7C};
then the information for 0x7D does not come.
Even if we rearrange the order as  {0x7F, 0x7E, , 0x7C, 0x7D}, the information of motor_ids[2] seems not coming, because the information of 0x7C is not coming.


Our pseudo code is as follows.

````
#include “cybergear_controller.hh”
#ifdef USE_ESP32_CAN
#include “cybergear_can_interface_esp32.hh”
#else
#include “cybergear_can_interface_mcp.hh”
#endif

std::vector<uint8_t> motor_ids = {0x7F, 0x7E, 0x7D, 0x7C};

// init cybergeardriver
CybergearController controller = CybergearController(MASTER_CAN_ID);
#ifdef USE_ESP32_CAN
CybergearCanInterfaceEsp32 interface;.
#else
CybergearCanInterfaceMcp interface; #else
#endif
~~
void setup()
{
  ~~
  interface.init();
  controller.init(motor_ids, MODE_CURRENT, &interface);
  controller.enable_motors(); controller.
  ~~
}

void loop() {
~~
  std::vector<MotorStatus> status_list(NUM_MOTORS);
  if (controller.process_packet()) {
    controller.get_motor_status(status_list)) 
 }

~~
}
````

Our environment is,
M5stack Grey+ M5Stack Commu module
M5stack Grey+ M5Stack Commu module.
We have confirmed that the bilateral demo and other things work fine.

Do you have any idea what is wrong?

Thanks


Discussion / hints:
Hi @TakeruAce, thank you for your comment.

The M5Stack CommuModule uses MCP2515 as a communication IC. This IC is connected to the I2C pins of the M5Stack. However, due to the limitations of the MCP2515 library connected via I2C, hardware interrupts cannot be used during data receivement. As a result, an additional 250 microseconds of waiting time must be introduced for CAN response packet. This wait time corresponds to the time it takes for Cybergear to return a response packet.

Please modify your code as follows.


**Before**

```cpp
  controller.init(motor_ids, MODE_CURRENT, &interface);
```

**After**

```cpp
  // wait 250usec after sending packet
  controller.init(motor_ids, MODE_CURRENT, &interface, 250);
```
Thank you for your rapid reaction.

```cpp
  // wait 250usec after sending packet
  controller.init(motor_ids, MODE_CURRENT, &interface, 250);
```
I made the above change, but the issue has not been resolved. For good measure, I extended the timeout to 5000, but there is still no improvement.

I would also like to report some additional findings. For instance, when I set:

```cpp
std::vector<uint8_t> motor_ids = {0x7F, 0x7E, 0x7D, 0x7C, 0x7B, 0x7A};
```
The data for 0x7D, 0x7C, and 0x7B are not coming through, so it seems I can only read data for motors 0, 1, and the last one. Additionally, when reading motor statuses for more than four motors, the data for the 2nd to (n-1)th motors fail, but commands such as position and current for more than four motors are executed correctly.

I suspect that this kind of data loss may be due to a buffer overflow occurring somewhere in the data exchange process. However, I have not been able to pinpoint where exactly this issue is happening.

Thanks
Hi @chikuta ,

After modifying the code to handle each motor's write and read operation sequentially and add 250us wait when controller initialization, the issue was resolved. Here's the modification that worked:

```cpp
void init()
{
  controller.init(motor_ids, MODE_CURRENT, &interface, 250);  // This line is essential
}
void update()
{
  std::vector<MotorStatus> status_list(NUM_MOTORS);
  for (int i = 0; i < NUM_MOTORS; i++) {
    controller.send_position_command(motor_ids[i], positions[i]);
    if (controller.process_packet()) {
      MotorStatus status;
      controller.get_motor_status(motor_ids[i], status);
      status_list[i] = status;
      Serial.print(status.position);
      Serial.print(",");
    }
  }
}
```

Let me know if you have any insights into why the issue occurred with simultaneous commands, or if you think there's a better way to address this.
Thanks, 

HI @TakeruAce 

Ah, I see. I've identified the cause of the issue. It's a problem with data transmission that doesn't handle interrupts. I will address it fundamentally later, but please try modifying the code below in the meantime.

https://github.com/project-sternbergia/cybergear_m5/blob/2354223dbf6b35a54918c77d58de260385c8f67f/src/cybergear_controller.cpp#L222-L233

to

```cpp
bool CybergearController::send_position_command(
  const std::vector<uint8_t> ids, const std::vector<float> positions)
{
  // check size
  if (ids.size() != positions.size()) return false;

  bool ret = true;
  for (uint8_t idx = 0; idx < ids.size(); ++idx) {
    ret &= send_position_command(ids[idx], positions[idx]);
    process_packet();
  }
  return ret;
}
```

Fix patch:
diff --git a/src/cybergear_can_interface.hh b/src/cybergear_can_interface.hh
index 8556fed..b97dca6 100644
--- a/src/cybergear_can_interface.hh
+++ b/src/cybergear_can_interface.hh
@@ -11,6 +11,7 @@ public:
   virtual bool send_message(uint32_t id, const uint8_t * data, uint8_t len, bool ext) = 0;
   virtual bool read_message(unsigned long & id, uint8_t * data, uint8_t & len) = 0;
   virtual bool available() = 0;
+  virtual bool support_interrupt() = 0;
 };
 
 #endif  // CYBERGEAR_CAN_INTERFACE_HH
diff --git a/src/cybergear_can_interface_esp32.cpp b/src/cybergear_can_interface_esp32.cpp
index a779c05..175458a 100644
--- a/src/cybergear_can_interface_esp32.cpp
+++ b/src/cybergear_can_interface_esp32.cpp
@@ -92,3 +92,5 @@ bool CybergearCanInterfaceEsp32::available()
   CG_DEBUG_FUNC
   return (buffer.isEmpty() == false);
 }
+
+bool CybergearCanInterfaceEsp32::support_interrupt() { return true; }
diff --git a/src/cybergear_can_interface_esp32.hh b/src/cybergear_can_interface_esp32.hh
index b2dc851..ffeedd6 100644
--- a/src/cybergear_can_interface_esp32.hh
+++ b/src/cybergear_can_interface_esp32.hh
@@ -21,6 +21,7 @@ public:
   virtual bool send_message(uint32_t id, const uint8_t * data, uint8_t len, bool ext);
   virtual bool read_message(unsigned long & id, uint8_t * data, uint8_t & len);
   virtual bool available();
+  virtual bool support_interrupt();
 
 private:
   uint8_t receive_buffer_[64];  //!< receive buffer
diff --git a/src/cybergear_can_interface_mcp.cpp b/src/cybergear_can_interface_mcp.cpp
index c46ec09..26b56a0 100644
--- a/src/cybergear_can_interface_mcp.cpp
+++ b/src/cybergear_can_interface_mcp.cpp
@@ -50,3 +50,5 @@ bool CybergearCanInterfaceMcp::available()
   }
   return true;
 }
+
+bool CybergearCanInterfaceMcp::support_interrupt() { return false; }
diff --git a/src/cybergear_can_interface_mcp.hh b/src/cybergear_can_interface_mcp.hh
index 58c3829..37d7040 100644
--- a/src/cybergear_can_interface_mcp.hh
+++ b/src/cybergear_can_interface_mcp.hh
@@ -17,6 +17,7 @@ public:
   virtual bool send_message(uint32_t id, const uint8_t * data, uint8_t len, bool ext);
   virtual bool read_message(unsigned long & id, uint8_t * data, uint8_t & len);
   virtual bool available();
+  virtual bool support_interrupt();
 
 private:
   MCP_CAN * pcan_ = nullptr;
diff --git a/src/cybergear_controller.cpp b/src/cybergear_controller.cpp
index a5c53df..5fd94e8 100644
--- a/src/cybergear_controller.cpp
+++ b/src/cybergear_controller.cpp
@@ -65,6 +65,9 @@ bool CybergearController::set_run_mode(
   bool ret = true;
   for (uint8_t idx = 0; idx < ids.size(); ++idx) {
     ret &= set_run_mode(ids[idx], modes[idx]);
+    if (!can_->support_interrupt()) {
+      process_packet();
+    }
   }
   return ret;
 }
@@ -73,6 +76,9 @@ bool CybergearController::set_run_mode(uint8_t mode)
 {
   for (uint8_t idx = 0; idx < motor_ids_.size(); ++idx) {
     drivers_[motor_ids_[idx]].set_run_mode(mode);
+    if (!can_->support_interrupt()) {
+      process_packet();
+    }
   }
   return true;
 }
@@ -168,6 +174,9 @@ bool CybergearController::enable_motors()
 {
   for (uint8_t idx = 0; idx < motor_ids_.size(); ++idx) {
     drivers_[motor_ids_[idx]].enable_motor();
+    if (!can_->support_interrupt()) {
+      process_packet();
+    }
   }
   return true;
 }
@@ -183,6 +192,9 @@ bool CybergearController::reset_motors()
 {
   for (uint8_t idx = 0; idx < motor_ids_.size(); ++idx) {
     drivers_[motor_ids_[idx]].reset_motor();
+    if (!can_->support_interrupt()) {
+      process_packet();
+    }
   }
   return true;
 }
@@ -198,6 +210,9 @@ bool CybergearController::send_motion_command(
   bool ret = true;
   for (uint8_t idx = 0; idx < ids.size(); ++idx) {
     ret &= send_motion_command(ids[idx], cmds[idx]);
+    if (!can_->support_interrupt()) {
+      process_packet();
+    }
   }
   return ret;
 }
@@ -228,6 +243,9 @@ bool CybergearController::send_position_command(
   bool ret = true;
   for (uint8_t idx = 0; idx < ids.size(); ++idx) {
     ret &= send_position_command(ids[idx], positions[idx]);
+    if (!can_->support_interrupt()) {
+      process_packet();
+    }
   }
   return ret;
 }
@@ -251,6 +269,9 @@ bool CybergearController::send_speed_command(
   bool ret = true;
   for (uint8_t idx = 0; idx < ids.size(); ++idx) {
     ret &= send_speed_command(ids[idx], speeds[idx]);
+    if (!can_->support_interrupt()) {
+      process_packet();
+    }
   }
   return ret;
 }
@@ -274,6 +295,9 @@ bool CybergearController::send_current_command(
   bool ret = true;
   for (uint8_t idx = 0; idx < ids.size(); ++idx) {
     ret &= send_current_command(ids[idx], currents[idx]);
+    if (!can_->support_interrupt()) {
+      process_packet();
+    }
   }
   return ret;
 }


Test patch: