Merge pull request #18 from ArendJan/mirte-master-dev

add hmc5883 compass + pid fallback

mirte_control/mirte_base_control/include/my_robot_hw_interface.h

@@ -53,30 +53,54 @@ class MyRobotHWInterface : public hardware_interface::RobotHW {
   MyRobotHWInterface();
 
   double calc_speed_pid(int joint, double target, const ros::Duration &period) {
+
+    // 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();
-
+      start[joint] = true;
       // Fix for dynamic reconfigure of all 4 PID controllers:
       auto g = this->reconfig_pid->getGains();
       if (!equal_gains(pid->getGains(), g)) {
         pid->setGains(g);
       }
       return 0;
     }
+
+    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");
+      return calc_speed_map(joint, target, period);
+    }
     auto curr_speed = vel[joint];
     auto err = target - curr_speed;
     auto pid_cmd = pid->computeCommand(err, period);
     return pid_cmd + _last_cmd[joint];
   }
 
+  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);
+  }
+
   bool write_single(int joint, double speed, const ros::Duration &period) {
     double speed_mapped;
     if (this->enablePID) {
       speed_mapped = this->calc_speed_pid(joint, speed, period);
     } else {
-      speed_mapped =
-          std::max(std::min(int(speed / (6 * M_PI) * 100), 100), -100);
+      speed_mapped = this->calc_speed_map(joint, speed, period);
     }
     speed_mapped = std::clamp<double>(speed_mapped, -max_speed, max_speed);
     auto diff = std::abs(speed_mapped - _last_sent_cmd[joint]);
@@ -170,6 +194,7 @@ class MyRobotHWInterface : public hardware_interface::RobotHW {
   double ticks = 40.0;
 
   std::vector<int> _wheel_encoder;
+  std::vector<ros::Time> _wheel_encoder_update_time;
   std::vector<double> _last_cmd;
   std::vector<double> _last_sent_cmd;
   std::vector<int> _last_value;
@@ -207,6 +232,7 @@ class MyRobotHWInterface : public hardware_interface::RobotHW {
       bidirectional = true;
     }
     _wheel_encoder[joint] = msg->value;
+    _wheel_encoder_update_time[joint] = msg->header.stamp;
   }
 
   // Thread and function to restart service clients when the service server has
@@ -271,6 +297,7 @@ MyRobotHWInterface::MyRobotHWInterface()
   // Initialize raw data
   for (size_t i = 0; i < NUM_JOINTS; i++) {
     _wheel_encoder.push_back(0);
+    _wheel_encoder_update_time.push_back(ros::Time::now());
     _last_value.push_back(0);
     _last_wheel_cmd_direction.push_back(0);
     _last_cmd.push_back(0);

mirte_msgs/CMakeLists.txt

@@ -58,6 +58,7 @@ add_message_files(
    IntensityDigital.msg
    Keypad.msg
    ServoPosition.msg
+   Heading.msg
 )
 
 ## Generate services in the 'srv' folder

mirte_msgs/msg/Heading.msg

@@ -0,0 +1,3 @@
+float32 x
+float32 y
+float32 z

mirte_telemetrix/config/mirte_master_config_goede_arm.yaml

@@ -180,4 +180,4 @@ neopixel:
   #   id: 0x41
   #   min_voltage: 11
   #   max_current: 1
-  #   max_voltage: 15
+  #   max_voltage: 15

mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py

@@ -21,6 +21,7 @@
 
 from modules import MPU9250
 from modules import Oled
+from modules import HMC5883L
 
 # NOTE: This call was unused
 # devices = rospy.get_param("mirte/device")
@@ -968,7 +969,16 @@ def add_modules(modules: dict, device: dict) -> []:
         if module["type"].lower() == "mpu9250":
             imu_module = MPU9250.MPU9250(board, module_name, module, board_mapping)
             tasks.append(loop.create_task(imu_module.start()))
-
+        if module["type"].lower().startswith("hmc5883"):
+            hmc_module = HMC5883L.HMC5883(
+                board,
+                module_name,
+                module,
+                board_mapping,
+                global_data=global_data,
+                loop=loop,
+            )
+            tasks.append(loop.create_task(hmc_module.start()))
     return tasks
 
 

mirte_telemetrix/scripts/modules/HMC5883L.py

@@ -0,0 +1,61 @@
+import time
+
+import rospy
+from mirte_msgs.msg import Heading
+import subprocess
+import asyncio
+
+
+class HMC5883:
+    def __init__(
+        self,
+        board,
+        module_name,
+        module,
+        board_mapping,
+        loop,
+        global_data,  # obj with some shared data, like SOC
+    ):
+        self.board = board
+        self.sensor = module
+        self.name = module_name
+        self.loop = loop
+        self.global_data = global_data
+        self.init_awaits = []
+        self.enabled = True
+        if board_mapping.get_mcu() == "pico":
+            if "connector" in self.sensor:
+                pins = board_mapping.connector_to_pins(self.sensor["connector"])
+            else:
+                pins = self.sensor["pins"]
+            pin_numbers = {}
+            for item in pins:
+                pin_numbers[item] = board_mapping.pin_name_to_pin_number(pins[item])
+            self.i2c_port = board_mapping.get_I2C_port(pin_numbers["sda"])
+            self.init_awaits.append(
+                self.board.set_pin_mode_i2c(
+                    i2c_port=self.i2c_port,
+                    sda_gpio=pin_numbers["sda"],
+                    scl_gpio=pin_numbers["scl"],
+                )
+            )
+        else:
+            print("hmc module probably not supported on your hardware!!!!")
+            self.init_awaits.append(self.board.set_pin_mode_i2c(i2c_port=self.i2c_port))
+        self.pub = rospy.Publisher(
+            "mirte/compass/" + self.name, Heading, queue_size=1, latch=True
+        )
+
+    async def start(self):
+        for ev in self.init_awaits:
+            try:  # catch set_pin_mode_i2c already for this port
+                z = await ev
+            except Exception as e:
+                pass
+        await asyncio.sleep(3)
+        await self.board.sensors.add_hmc5883(self.i2c_port, self.callback)
+
+    async def callback(self, data):
+        # TODO: this takes the raw heading values, offset is not removed from it
+        msg = Heading(data[0], data[1], data[2])
+        self.pub.publish(msg)