[Spinal][Servo] fix some bugs

sugihara-16 · Mar 6, 2024 · 0adcf56 · 0adcf56
1 parent bfcabad
commit 0adcf56
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 9 deletions.
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.cpp
@@ -123,14 +123,27 @@ void DirectServo::servoTorqueControlCallback(const spinal::ServoTorqueCmd& contr
 
 void DirectServo::servoConfigCallback(const spinal::SetDirectServoConfig::Request& req, spinal::SetDirectServoConfig::Response& res)
 {
+
+  uint8_t command = req.command;
+
+  /* special case : data[0] is flag value */
+  if(command == spinal::SetDirectServoConfig::Request::SET_DYNAMIXEL_TTL_RS485_MIXED)
+    {
+      servo_handler_.setTTLRS485Mixed(req.data[0]);
+      FlashMemory::erase();
+      FlashMemory::write();
+      res.success = true;
+      return;
+    }
+
   uint8_t servo_index = req.data[0];
   ServoData& s = servo_handler_.getOneServo(servo_index);
   if(s == servo_handler_.getOneServo(0)){ 
     nh_->logerror("Invalid Servo ID!");
     return;
   }
 
-  switch (req.command) {
+  switch (command) {
   case spinal::SetDirectServoConfig::Request::SET_SERVO_HOMING_OFFSET:
     {
       int32_t calib_value = req.data[1];
@@ -169,13 +182,6 @@ void DirectServo::servoConfigCallback(const spinal::SetDirectServoConfig::Reques
       servo_handler_.setCurrentLimit(servo_index);
       break;
     }
-  case spinal::SetDirectServoConfig::Request::SET_DYNAMIXEL_TTL_RS485_MIXED:
-    {
-      servo_handler_.setTTLRS485Mixed(req.data[0]); // special case -> data[0] is flag value
-      FlashMemory::erase();
-      FlashMemory::write();
-      break;
-    }
   case spinal::SetDirectServoConfig::Request::SET_SERVO_EXTERNAL_ENCODER_FLAG:
     {
       if(!s.torque_enable_){

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.h
@@ -42,7 +42,7 @@ class DirectServo
   void setGoalAngle(const std::map<uint8_t, float>& servo_map);
 
   uint32_t rad2Pos(float angle, float scale, uint32_t zero_point_pos){
-    return static_cast<uint32_t>(zero_point_pos - angle /scale);
+    return static_cast<uint32_t>(angle /scale + zero_point_pos);
   }
 
 private: