[Servo][Spinal][DJI M2006] calculate the angle and velocity in the ou…

…tput axis, and also publish these values
jsk-ros-pkg · Jan 13, 2025 · f4c3969 · f4c3969
1 parent 769abf0
commit f4c3969
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 7 deletions.
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/DJI_M2006/servo.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/DJI_M2006/servo.cpp
@@ -16,8 +16,13 @@ Servo::Servo(): servo_state_pub_("servo/states", &servo_state_msg_), servo_cmd_s
 
   rotations_ = 0;
   last_pos_measurement_ = kCountsPerRev;
+
   counts_ = 0;
   rpm_ = 0;
+  m_current_ = 0;
+
+  output_ang_ = 0;
+  output_vel_ = 0;
   current_ = 0;
 
   goal_current_ = 0;
@@ -51,7 +56,7 @@ void Servo::receiveDataCallback(uint32_t identifier, uint32_t dlc, uint8_t* data
 {
   uint16_t counts = uint16_t((data[0] << 8) | data[1]);
   rpm_ = int16_t((data[2] << 8) | data[3]);
-  current_ = int16_t((data[4] << 8) | data[5]);
+  m_current_ = int16_t((data[4] << 8) | data[5]);
 
   if (last_pos_measurement_ == 8192) {
     last_pos_measurement_ = counts;
@@ -70,6 +75,10 @@ void Servo::receiveDataCallback(uint32_t identifier, uint32_t dlc, uint8_t* data
 
   counts_ = rotations_ * kCountsPerRev + counts;
   last_pos_measurement_ = counts;
+
+  output_ang_ = counts_ / kCountsPerRad; // TODO: rectify the offset;
+  output_vel_ = rpm_ / kRPMPerRadS;
+  current_ = m_current_ / 1000.0f;
 }
 
 void Servo::update(void)
@@ -90,8 +99,8 @@ void Servo::update(void)
     {
       /* send servo */
       servo_state_msg_.index = 0;
-      servo_state_msg_.angle = counts_;
-      servo_state_msg_.rpm = rpm_;
+      servo_state_msg_.angle = output_ang_;
+      servo_state_msg_.velocity = output_vel_;
       servo_state_msg_.current = current_;
 
       servo_state_pub_.publish(&servo_state_msg_);

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/DJI_M2006/servo.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/DJI_M2006/servo.h
@@ -57,7 +57,11 @@ namespace DJI_M2006
     int32_t last_pos_measurement_;
     int32_t counts_;
     int32_t rpm_;
-    int32_t current_;
+    int32_t m_current_;
+
+    float output_ang_;
+    float output_vel_;
+    float current_;
 
     int16_t goal_current_;
 

diff --git a/aerial_robot_nerve/spinal/msg/ServoExtendedState.msg b/aerial_robot_nerve/spinal/msg/ServoExtendedState.msg
@@ -1,5 +1,5 @@
 uint8 index # from 0
-int32 angle
-int32 rpm
-int32 current # load
+float32 angle # [rad]
+float32 velocity # [rad/s]
+float32 current # [A]