Merge pull request #40 from chikuta/fix-mcp-multi-communication

Fix mcp multi communication
project-sternbergia · Oct 7, 2024 · ec53eb3 · ec53eb3
2 parents 2354223 + 624d0c8
commit ec53eb3
Show file tree

Hide file tree

Showing 6 changed files with 31 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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;
 }
-Original file line number
+Diff line change
@@ Expand Up / @@ -50,3 +50,5 @@ bool CybergearCanInterfaceMcp::available() @@
       }
       return true;
     }
+    bool CybergearCanInterfaceMcp::support_interrupt() { return false; }