style fix pid fallback

ArendJan · Jun 10, 2024 · 5986ceb · 5986ceb
1 parent a74aefe
commit 5986ceb
Showing 1 changed file with 15 additions and 6 deletions.
diff --git a/mirte_control/mirte_base_control/include/my_robot_hw_interface.h b/mirte_control/mirte_base_control/include/my_robot_hw_interface.h
@@ -53,7 +53,10 @@ class MyRobotHWInterface : public hardware_interface::RobotHW {
   MyRobotHWInterface();
 
   double calc_speed_pid(int joint, double target, const ros::Duration &period) {
-    static bool start[4] = {true}; // if moving from 0 to something else, don't wait for the PID to catch up, but use the mapping function to immediately give it a kinda okay value  
+
+    // if moving from 0 to something else, don't wait for the PID to catch up,
+    // but use the mapping function to immediately give it a kinda okay value
+    static bool start[4] = {true};
     auto pid = this->pids[joint];
     if (target == 0) {
       pid->reset();
@@ -66,14 +69,20 @@ class MyRobotHWInterface : public hardware_interface::RobotHW {
       return 0;
     }
 
-    if(start[joint]) {
+    if (start[joint]) {
       start[joint] = false;
       return calc_speed_map(joint, target, period);
     }
 
-    auto diff_enc_time_upd = ros::Time::now() - _wheel_encoder_update_time[joint];
-    if(diff_enc_time_upd > ros::Duration(1,0) && _last_cmd[joint] > 30) { // if the motors don't move, no need to fall back yet
-      ROS_WARN_STREAM("Encoder " << joint << " not reporting data, falling back to mapping calculation");
+    auto diff_enc_time_upd =
+        ros::Time::now() - _wheel_encoder_update_time[joint];
+    if (diff_enc_time_upd > ros::Duration(1, 0) &&
+        _last_cmd[joint] >
+            30) { // if the motors don't move, no need to fall back yet
+      ROS_WARN_STREAM(
+          "Encoder "
+          << joint
+          << " not reporting data, falling back to mapping calculation");
       return calc_speed_map(joint, target, period);
     }
     auto curr_speed = vel[joint];
@@ -82,7 +91,7 @@ class MyRobotHWInterface : public hardware_interface::RobotHW {
     return pid_cmd + _last_cmd[joint];
   }
 
-  double calc_speed_map(int joint, double target, const ros::Duration & period) {
+  double calc_speed_map(int joint, double target, const ros::Duration &period) {
     return std::max(std::min(int(target / (6.0 * M_PI) * 100), 100), -100);
   }