fix control mode can msg frame

CaptainKAZ · Jul 10, 2022 · 52b8114 · 52b8114
1 parent 95eb7c4
commit 52b8114
Show file tree

Hide file tree

Showing 6 changed files with 55 additions and 22 deletions.
diff --git a/...ple_hardware_interface/include/odrive_cansimple_hardware_interface/cansimple_protocol.hpp b/...ple_hardware_interface/include/odrive_cansimple_hardware_interface/cansimple_protocol.hpp
@@ -16,6 +16,35 @@ namespace odrive_cansimple_hardware_interface
 class CanSimpleAxis
 {
 public:
+  enum
+  {
+    MSG_CO_NMT_CTRL = 0x000,  // CANOpen NMT Message REC
+    MSG_ODRIVE_HEARTBEAT,
+    MSG_ODRIVE_ESTOP,
+    MSG_GET_MOTOR_ERROR,  // Errors
+    MSG_GET_ENCODER_ERROR,
+    MSG_GET_SENSORLESS_ERROR,
+    MSG_SET_AXIS_NODE_ID,
+    MSG_SET_AXIS_REQUESTED_STATE,
+    MSG_SET_AXIS_STARTUP_CONFIG,
+    MSG_GET_ENCODER_ESTIMATES,
+    MSG_GET_ENCODER_COUNT,
+    MSG_SET_CONTROLLER_MODES,
+    MSG_SET_INPUT_POS,
+    MSG_SET_INPUT_VEL,
+    MSG_SET_INPUT_TORQUE,
+    MSG_SET_VEL_LIMIT,
+    MSG_START_ANTICOGGING,
+    MSG_SET_TRAJ_VEL_LIMIT,
+    MSG_SET_TRAJ_ACCEL_LIMITS,
+    MSG_SET_TRAJ_INERTIA,
+    MSG_GET_IQ,
+    MSG_GET_SENSORLESS_ESTIMATES,
+    MSG_RESET_ODRIVE,
+    MSG_GET_VBUS_VOLTAGE,
+    MSG_CLEAR_ERRORS,
+    MSG_CO_HEARTBEAT_CMD = 0x700,  // CANOpen NMT Heartbeat  SEND
+  };
   void configure(double gear, double kt)
   {
     reduction_ratio_ = gear;
@@ -33,34 +62,35 @@ class CanSimpleAxis
   }
   void eStop()
   {
-    can_frame frame;
+    can_frame frame{};
     frame.can_id = can_id_ << 5 | 0x02;
     frame.can_dlc = 0;
     *can_device_ << frame;
   }
   void clearErrors()
   {
-    can_frame frame;
+    can_frame frame{};
     frame.can_id = can_id_ << 5 | 0x018;
     frame.can_dlc = 0;
     *can_device_ << frame;
   }
   // https://docs.odriverobotics.com/v/latest/fibre_types/com_odriverobotics_ODrive.html#ODrive.Axis.AxisState
-  void setAxisRequestedState(uint32_t state)
+  void setAxisRequestedState(uint16_t state)
   {
-    can_frame frame;
+    can_frame frame{};
     frame.can_id = can_id_ << 5 | 0x07;
-    frame.can_dlc = 4;
-    std::memcpy(frame.data, &state, 4);
+    frame.can_dlc = 2;
+    std::memcpy(frame.data, &state, 2);
     *can_device_ << frame;
   }
   // https://docs.odriverobotics.com/v/latest/fibre_types/com_odriverobotics_ODrive.html#ODrive.Controller.ControlMode
-  void setControllerMode(uint32_t mode)
+  void setControllerMode(uint32_t control_mode,uint32_t input_mode)
   {
-    can_frame frame;
+    can_frame frame{};
     frame.can_id = can_id_ << 5 | 0x0B;
-    frame.can_dlc = 4;
-    std::memcpy(frame.data, &mode, 4);
+    frame.can_dlc = 8;
+    std::memcpy(frame.data, &control_mode, 4);
+    std::memcpy(frame.data+4, &input_mode, 4);
     *can_device_ << frame;
   }
   void writeCommand()

diff --git a/odrive_cansimple_hardware_interface/src/cansimple_hardware_interface.cpp b/odrive_cansimple_hardware_interface/src/cansimple_hardware_interface.cpp
@@ -94,7 +94,7 @@ hardware_interface::return_type CanSimpleHardwareInterface::start()
 {
   for (auto& axis : axies_)
   {
-    axis.setControllerMode(0x03);
+    axis.setControllerMode(0x03,0x01);
     axis.setAxisRequestedState(0x08);
   }
   status_ = hardware_interface::status::STARTED;

diff --git a/odrive_cansimple_hardware_interface/src/socketcan.cpp b/odrive_cansimple_hardware_interface/src/socketcan.cpp
@@ -208,15 +208,18 @@ const SocketCan* SocketCan::operator>>(struct can_frame& frame)
   {
     RCUTILS_LOG_ERROR_NAMED("SocketCAN", "Can't receive frame, callback is set");
   }
-  std::lock_guard<std::mutex> lock(rxMutex_);
-  if (rxQueue_.empty())
-  {
-    memset(&frame, 0, sizeof(can_frame));
-  }
-  else
+  if (rxMutex_.try_lock())
   {
-    frame = rxQueue_.front();
-    rxQueue_.pop();
+    if (rxQueue_.empty())
+    {
+      memset(&frame, 0, sizeof(can_frame));
+    }
+    else
+    {
+      frame = rxQueue_.front();
+      rxQueue_.pop();
+    }
+    rxMutex_.unlock();
   }
   return this;
 }
diff --git a/odrive_cansimple_hardware_interface/test/pos_control.cpp b/odrive_cansimple_hardware_interface/test/pos_control.cpp
@@ -22,7 +22,7 @@ int main()
   std::cout << "can id?" << std::endl;
   std::cin >> id;
   axis.attach(can_device, id);
-  axis.setControllerMode(0x03);
+  axis.setControllerMode(0x03,0x01);
   axis.setAxisRequestedState(0x08);
   auto last_time = std::chrono::high_resolution_clock::now();
   // NOTE:may need to adjust

diff --git a/odrive_cansimple_hardware_interface/test/torque_control.cpp b/odrive_cansimple_hardware_interface/test/torque_control.cpp
@@ -18,7 +18,7 @@ int main(){
     std::cout<<"can id?"<<std::endl;
     std::cin>>id;
     axis.attach(can_device,id);
-    axis.setControllerMode(0x03);
+    axis.setControllerMode(0x03,0x01);
     axis.setAxisRequestedState(0x08);
     // auto last_time = std::chrono::high_resolution_clock::now();
     *axis.getCommandKpPtr()=0;

diff --git a/odrive_cansimple_hardware_interface/test/vel_control.cpp b/odrive_cansimple_hardware_interface/test/vel_control.cpp
@@ -22,7 +22,7 @@ int main()
   std::cout << "can id?" << std::endl;
   std::cin >> id;
   axis.attach(can_device, id);
-  axis.setControllerMode(0x03);
+  axis.setControllerMode(0x03,0x01);
   axis.setAxisRequestedState(0x08);
   // NOTE:may need to adjust
   *axis.getCommandKpPtr() = 20;