diff --git a/mirte_telemetrix/config/mirte_user_config.yaml b/mirte_telemetrix/config/mirte_user_config.yaml index a4de7df..3a6f739 100644 --- a/mirte_telemetrix/config/mirte_user_config.yaml +++ b/mirte_telemetrix/config/mirte_user_config.yaml @@ -2,7 +2,7 @@ device: mirte: type: breadboard mcu: pico - max_frequency: 100 + max_frequency: 10 distance: left: name: left @@ -10,7 +10,7 @@ distance: pins: trigger: 7 echo: 6 -motor: +encoder: left: name: left device: mirte diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 24435d0..463e19e 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -8,6 +8,8 @@ import signal import aiorospy import io +from inspect import signature + # Import the right Telemetrix AIO devices = rospy.get_param('/mirte/device') @@ -336,6 +338,7 @@ def get_data(self, req): return GetDistanceResponse(self.last_publish_value.range) async def start(self): + await self.board.set_scan_delay(100) await self.board.set_pin_mode_sonar(self.pins["trigger"], self.pins["echo"], self.publish_data) async def publish_data(self, data): @@ -407,7 +410,10 @@ def get_data(self, req): return GetEncoderResponse(self.last_publish_value.value) async def start(self): - await self.board.set_pin_mode_encoder(self.pins["pin"], 2, self.ticks_per_wheel, self.publish_data) + if("encoder_pin" in signature(self.board.set_pin_mode_encoder).parameters): + await self.board.set_pin_mode_encoder(self.pins["pin"], 2, self.ticks_per_wheel, self.publish_data) + else: + await self.board.set_pin_mode_encoder(self.pins["pin"], 0, self.publish_data, False) rospy.Timer(rospy.Duration(1.0/10.0), self.publish_speed_data) def publish_speed_data(self, event=None):