diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md index 257ac82f..2c82bcaa 100644 --- a/.github/pull_request_template.md +++ b/.github/pull_request_template.md @@ -17,9 +17,8 @@ before and after the PR.--> ## Checklist - [ ] Signed all commits for DCO - [ ] Added tests +- [ ] Added example and/or tutorial - [ ] Updated documentation (as needed) -- [ ] Updated migration guide (as needed) -- [ ] Consider updating Python bindings (if it affects the public API) **Note to maintainers**: Remember to use **Squash-Merge** and edit the commit message to match the pull request summary while retaining `Signed-off-by` messages. @@ -41,7 +40,5 @@ context (e.g., screenshots, gifs) if appropriate.--> - [ ] Added tests - [ ] Added example and/or tutorial - [ ] Updated documentation (as needed) -- [ ] Updated migration guide (as needed) -- [ ] Consider updating Python bindings (if it affects the public API) **Note to maintainers**: Remember to use **Squash-Merge** and edit the commit message to match the pull request summary while retaining `Signed-off-by` messages. diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 6c045d39..2cb1cf30 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -29,6 +29,6 @@ jobs: - uses: ros-tooling/action-ros-ci@v0.3 id: action_ros_ci_step with: - package-name: andino_base andino_bringup andino_control andino_description andino_firmware andino_hardware andino_slam andino_gz_classic andino_navigation + package-name: andino_base andino_bringup andino_control andino_description andino_firmware andino_hardware andino_slam andino_gz_classic andino_navigation andino_apps target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/mcu-ci.yml b/.github/workflows/mcu-ci.yml new file mode 100644 index 00000000..d4927c12 --- /dev/null +++ b/.github/workflows/mcu-ci.yml @@ -0,0 +1,63 @@ +# BSD 3-Clause License +# +# Copyright (c) 2023, Ekumen Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +name: MCU CI + +on: + push: + branches: + - humble + pull_request: + branches: + - humble + workflow_dispatch: + +jobs: + build_and_test: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + - uses: actions/cache@v3 + with: + path: | + ~/.cache/pip + ~/.platformio/.cache + key: ${{ runner.os }}-pio + - uses: actions/setup-python@v4 + with: + python-version: '3.9' + - name: Install PlatformIO Core + run: pip install --upgrade platformio + - name: Build application + working-directory: ./andino_firmware + run: pio run + - name: Run unit tests + working-directory: ./andino_firmware + run: pio test --environment=desktop diff --git a/.gitignore b/.gitignore index 3769a4ab..5c9f39c1 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,6 @@ # Visual Studio Code files .vscode + +# PyCache +*.pyc diff --git a/README.md b/README.md index ddff076d..3c62592f 100644 --- a/README.md +++ b/README.md @@ -28,16 +28,20 @@ _Note: For videos go to [Media](#selfie-media) section._ - :hammer_and_pick: [`andino_firmware`](./andino_firmware): Contains the code be run in the microcontroller for interfacing low level hardware with the SBC. - :gear: [`andino_base`](./andino_base): [ROS Control hardware interface](https://control.ros.org/master/doc/ros2_control/hardware_interface/doc/writing_new_hardware_interface.html) is implemented. - :control_knobs: [`andino_control`](./andino_control/): It launches the [`controller_manager`](https://control.ros.org/humble/doc/ros2_control/controller_manager/doc/userdoc.html) along with the [ros2 controllers](https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html): [diff_drive_controller](https://control.ros.org/master/doc/ros2_controllers/diff_drive_controller/doc/userdoc.html) and the [joint_state_broadcaster](https://control.ros.org/master/doc/ros2_controllers/joint_state_broadcaster/doc/userdoc.html). -- :computer: [`andino_gz_classic`](./andino_gz_classic/): Gazebo simulation of the `andino` robot. +- :computer: [`andino_gz_classic`](./andino_gz_classic/): [Gazebo Classic](https://classic.gazebosim.org/) simulation of the `andino` robot. - :world_map: [`andino_slam`](./andino_slam/): Provides support for SLAM with your `andino` robot. - :compass: [`andino_navigation`](./andino_navigation/): Navigation stack based on `nav2`. +- :exclamation: [`andino_apps`](./andino_apps/): Integrated applications with the `andino` robot. ## :paperclips: Related projects Other projects built upon Andino! :rocket: -- :test_tube: [`andino_integration_tests`](https://github.com/Ekumen-OS/andino_integration_tests): Extension to the Andino robot showing how to build integration tests. +- :computer: [`andino_gz`](https://github.com/Ekumen-OS/andino_gz): [Gazebo](https://gazebosim.org/home)(non-classic) simulation of the `andino` robot. - :lady_beetle: [`andino_webots`](https://github.com/Ekumen-OS/andino_webots): [Webots](https://github.com/cyberbotics/webots) simulation of the Andino robot fully integrated with ROS 2. +- :joystick: [`andino_o3de`](https://github.com/Ekumen-OS/andino_o3de): [O3DE](https://o3de.org/) simulation of the Andino robot. +- :green_circle: [`andino_isaac`](https://github.com/Ekumen-OS/andino_isaac): [Isaac Sim](https://docs.omniverse.nvidia.com/isaacsim/latest/index.html) simulation of the Andino robot. +- :test_tube: [`andino_integration_tests`](https://github.com/Ekumen-OS/andino_integration_tests): Extension to the Andino robot showing how to build integration tests. ## :pick: Robot Assembly @@ -104,6 +108,12 @@ source install/setup.bash `Note`: Whether your are installing the packages in your dev machine or in your robot the procedure is the same. +### Install the binaries + +The packages have been also released via ROS package manager system for the 'humble' distro. You can check them [here](https://repo.ros2.org/status_page/ros_humble_default.html?q=andino). + +These packages can be installed using `apt` (e.g: `sudo apt install ros-humble-andino-description`) or using `rosdep`. + ## :rocket: Usage ### Robot bringup @@ -122,17 +132,38 @@ By default sensors like the camera and the lidar are initialized. This can be di - include_rplidar: `true` as default. - include_camera: `true` as default. -After the robot is launched, use `ROS 2 CLI` for inspecting environment. E.g: By doing `ros2 topic list` the available topics can be displayed. +After the robot is launched, use `ROS 2 CLI` for inspecting environment. +For example, by doing `ros2 topic list` the available topics can be displayed: + + /camera_info + /cmd_vel + /image_raw + /odom + /robot_description + /scan + /tf + /tf_static + + _Note: Showing just some of them_ ### Teleoperation Launch files for using the keyboard or a joystick for teleoperating the robot are provided. -[`twist_mux`](http://wiki.ros.org/twist_mux) is used to at the same time accept command velocities from different topics using certain priority for each one of them (See [twist_mux config](andino_bringup/config/twist_mux.yaml)). Available topics are (ordering by priority): +#### Keyboard + +``` +ros2 launch andino_bringup teleop_keyboard.launch.py +``` +This is similarly to just executing `ros2 run teleop_twist_keyboard teleop_twist_keyboard`. + +#### Joystick -- cmd_vel -- cmd_vel_keyboard -- cmd_vel_joy +Using a joystick for teleoperating is notably better. +You need the joystick configured as explained [here](andino_hardware/README.md#Using-joystick-for-teleoperation). +``` +ros2 launch andino_bringup teleop_joystick.launch.py +``` ### RViz @@ -144,20 +175,19 @@ ros2 launch andino_bringup rviz.launch.py For starting `rviz2` visualization with a provided configuration. -## :computer: Simulation - -The [`andino_gz_classic`](./andino_gz_classic/README.MD) package provides a Gazebo simulation fo the Andino robot. - - - ## :compass: Navigation The [`andino_navigation`](./andino_navigation/README.md) package provides a navigation stack based on the great [Nav2](https://github.com/ros-planning/navigation2) package. https://github.com/Ekumen-OS/andino/assets/53065142/29951e74-e604-4a6e-80fc-421c0c6d8fee -_Important!: At the moment this package is only working with the simulation. The support for the real robot is forthcoming._ +Follow the [`andino_navigation`'s README](./andino_navigation/README.md) instructions for bringing up the Navigation stack in the real robot or in the simulation. + +## :computer: Simulation +The [`andino_gz_classic`](./andino_gz_classic/README.MD) package provides a Gazebo simulation for the Andino robot. + + ## :selfie: Media @@ -192,7 +222,7 @@ This section is dedicated to recognizing and expressing gratitude to the open-so ## :raised_hands: Contributing -Issues or PRs are always welcome! Please refer to [CONTRIBUTING](CONTRIBUTING.md) doc. +Issues or PRs are always welcome! Please refer to [CONTRIBUTING](CONTRIBUTING.md) doc. ## Code development diff --git a/andino_apps/CHANGELOG.rst b/andino_apps/CHANGELOG.rst new file mode 100644 index 00000000..9d89418e --- /dev/null +++ b/andino_apps/CHANGELOG.rst @@ -0,0 +1,9 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package andino_apps +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2024-04-10) +------------------ +* Initial version of the package +* Moved launch file to execute Gazebo classic simulation + Nav2 from `andino_navigation` (`#228 `_) +* Contributors: Jesús Silva diff --git a/andino_apps/CMakeLists.txt b/andino_apps/CMakeLists.txt new file mode 100644 index 00000000..4ace334f --- /dev/null +++ b/andino_apps/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.8) +project(andino_apps) + +# find dependencies +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY + launch + DESTINATION + share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/andino_apps/LICENSE b/andino_apps/LICENSE new file mode 100644 index 00000000..40c6f118 --- /dev/null +++ b/andino_apps/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2024, Ekumen Inc. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/andino_apps/README.md b/andino_apps/README.md new file mode 100644 index 00000000..ae1e7342 --- /dev/null +++ b/andino_apps/README.md @@ -0,0 +1,28 @@ +# Andino Apps + +This package contains integration applications with the Andino robot. + +# Applications + +## Gazebo classic simulation + Nav2 + +A launch file for running the andino_gz_classic simulation and the Nav 2 stack is provided. +It uses the [turtlebot3_world](https://github.com/ROBOTIS-GIT/turtlebot3_simulations/tree/master) world (_Apache 2 license_) by default. + +``` + ros2 launch andino_apps andino_simulation_navigation.launch.py +``` + +To visualize and interact with the Andino robot in RViz: + +- Click in 2D pose estimate button and select the initial pose of the robot +- Click in Nav2 Goal button and select the final point. +- The robot will start to move to the selected goal. + +![Rviz_example_Nav2](docs/Rviz_example_Nav2.gif) + +For further information and examples you can check the [Nav2 tutorials](https://navigation.ros.org/tutorials/index.html). + +This package has been tested with the Andino robot with `diff drive plugin` in Gazebo-classic. + +By changing the world file, make sure to also change map file. Further navigation [parameters](params/nav2_params.yaml) tunning is recommended. diff --git a/andino_apps/docs/Rviz_example_Nav2.gif b/andino_apps/docs/Rviz_example_Nav2.gif new file mode 100644 index 00000000..2697f2a9 Binary files /dev/null and b/andino_apps/docs/Rviz_example_Nav2.gif differ diff --git a/andino_navigation/launch/andino_simulation_navigation.launch.py b/andino_apps/launch/andino_simulation_navigation.launch.py similarity index 100% rename from andino_navigation/launch/andino_simulation_navigation.launch.py rename to andino_apps/launch/andino_simulation_navigation.launch.py diff --git a/andino_apps/package.xml b/andino_apps/package.xml new file mode 100644 index 00000000..647e42f2 --- /dev/null +++ b/andino_apps/package.xml @@ -0,0 +1,25 @@ + + + + andino_apps + 0.1.0 + Package for apps created with andino + JesusSilvaUtrera + JesusSilvaUtrera + Franco Cipollone + BSD Clause 3 + + ament_cmake + + ros2launch + andino_gz_classic + nav2_bringup + rviz2 + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/andino_bringup/README.md b/andino_bringup/README.md new file mode 100644 index 00000000..92e1ea0c --- /dev/null +++ b/andino_bringup/README.md @@ -0,0 +1,26 @@ +# andino_bringup + +## Description +This package contains mainly launch files in order to launch all related driver and nodes to be used in the real robot. Some configuration files are also added to set up the nodes launched in the 'config' folder. + +## Launch Files + +### Main file + +The main launch file is the `andino_robot.launch.py` file. This file executes all necessary nodes for Andino Robot to work. + +It includes: + +- `andino_description.launch.py`: file to load the 3D model of the robot from the URDF. +- `andino_control.launch.py`: file for the `ros2_control` nodes and drivers. +- `camera.launch.py`: file to execute the camera drivers, using `v4l2_camera` package. If not necessary, it can be disabled using the `include_camera` parameter. +- `rplidar.launch.py`: file to execute the drivers of the RP 2D LiDAR attached to the robot. If not necessary, it can be disabled using the `include_rplidar` parameter. + +### Other launch files included + +These launch files are included just in case you want to command the robot in different ways, or visualize the information: + +- `rosbag_record.launch.py`: executes the `ros2 bag record` command to store a '.bag' file with the specified topics. If no topics are specified, it will record all of them. +- `rviz.launch.py`: executes the RViz visualization tool with the 'andino.rviz' configuration by default, to be able to visualize the 3D model of the robot and information from the topics. +- `teleop_joystick.launch.py`: this file allows you to command the robot using a controller with a joystick, using the `teleop_twist_joy` and the `joy_linux` packages. +- `teleop_keyboard.launch.py`: this file allows you to command the robot using a keyboard, using the `teleop_twist_keyboard` package. diff --git a/andino_bringup/launch/andino_robot.launch.py b/andino_bringup/launch/andino_robot.launch.py index f04317dd..1c1f1cb7 100644 --- a/andino_bringup/launch/andino_robot.launch.py +++ b/andino_bringup/launch/andino_robot.launch.py @@ -103,14 +103,6 @@ def generate_launch_description(): rplidar_timer = TimerAction(period=3.0, actions=[include_rplidar]) camera_timer = TimerAction(period=3.0, actions=[include_camera]) - twist_mux_params = os.path.join(pkg_andino_bringup,'config','twist_mux.yaml') - twist_mux = Node( - package="twist_mux", - executable="twist_mux", - parameters=[twist_mux_params], - remappings=[('/cmd_vel_out','/diff_controller/cmd_vel_unstamped')] - ) - return LaunchDescription([ include_andino_description, andino_control_timer, @@ -118,5 +110,4 @@ def generate_launch_description(): camera_timer, rplidar_arg, rplidar_timer, - twist_mux, ]) diff --git a/andino_bringup/launch/teleop_joystick.launch.py b/andino_bringup/launch/teleop_joystick.launch.py index 9a099b39..22dfda2c 100644 --- a/andino_bringup/launch/teleop_joystick.launch.py +++ b/andino_bringup/launch/teleop_joystick.launch.py @@ -42,7 +42,7 @@ def generate_launch_description(): cmd_vel_topic_arg = DeclareLaunchArgument( 'cmd_vel_topic', - default_value='/cmd_vel_joy', + default_value='/cmd_vel', description='Indicates the cmd_vel topic.') cmd_vel_topic = LaunchConfiguration('cmd_vel_topic') diff --git a/andino_bringup/launch/teleop_keyboard.launch.py b/andino_bringup/launch/teleop_keyboard.launch.py index cf748b76..1139dcec 100644 --- a/andino_bringup/launch/teleop_keyboard.launch.py +++ b/andino_bringup/launch/teleop_keyboard.launch.py @@ -36,7 +36,6 @@ def generate_launch_description(): package='teleop_twist_keyboard', executable='teleop_twist_keyboard', name='teleop_twist_keyboard_node', - remappings=[('/cmd_vel','/cmd_vel_keyboard')], output='screen', prefix = 'xterm -e', ) diff --git a/andino_bringup/package.xml b/andino_bringup/package.xml index 9bde80db..fafb1079 100644 --- a/andino_bringup/package.xml +++ b/andino_bringup/package.xml @@ -17,10 +17,10 @@ rplidar_ros teleop_twist_joy joy_linux - twist_mux v4l2_camera laser_filters rosbag2_storage_mcap + xterm ament_lint_auto ament_lint_common diff --git a/andino_control/README.md b/andino_control/README.md index 42b7c0e3..f14d9dc0 100644 --- a/andino_control/README.md +++ b/andino_control/README.md @@ -23,8 +23,8 @@ flowchart TD M(Controller Manager) -..-> |manages lifecycle| C M(Controller Manager) -..-> |activates hw component| B - D["/diff_controller/cmd_vel"] -->|subs| A - A -->|pubs| O["/diff_controller/odom"] + D["/cmd_vel"] -->|subs| A + A -->|pubs| O["/odom"] A -->|pubs| T["/tf"] C -->|pubs| J["/joint_states"] diff --git a/andino_control/config/andino_controllers.yaml b/andino_control/config/andino_controllers.yaml index 2f73e78f..2da1e4fe 100644 --- a/andino_control/config/andino_controllers.yaml +++ b/andino_control/config/andino_controllers.yaml @@ -21,6 +21,7 @@ imu_sensor_broadcaster: diff_controller: ros__parameters: + # See https://github.com/ros-controls/ros2_controllers/blob/humble/diff_drive_controller/src/diff_drive_controller_parameter.yaml publish_rate: 50.0 left_wheel_names: ['left_wheel_joint'] right_wheel_names: ['right_wheel_joint'] diff --git a/andino_control/launch/andino_control.launch.py b/andino_control/launch/andino_control.launch.py index 8b41d3fc..51a431b0 100644 --- a/andino_control/launch/andino_control.launch.py +++ b/andino_control/launch/andino_control.launch.py @@ -54,7 +54,12 @@ def generate_launch_description(): executable="ros2_control_node", parameters=[{'robot_description': ParameterValue(robot_description, value_type=str)}, controller_params_file], - + remappings=[ + ('/diff_controller/cmd_vel', '/cmd_vel'), # Used if use_stamped_vel param is true + ('/diff_controller/cmd_vel_unstamped', '/cmd_vel'), # Used if use_stamped_vel param is false + ('/diff_controller/cmd_vel_out', '/cmd_vel_out'), # Used if publish_limited_velocity param is true + ('/diff_controller/odom', '/odom'), + ], output="both", ) diff --git a/andino_description/CMakeLists.txt b/andino_description/CMakeLists.txt index 16b4becc..0eacb21b 100644 --- a/andino_description/CMakeLists.txt +++ b/andino_description/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.8) project(andino_description) find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) install( DIRECTORY @@ -14,4 +15,19 @@ install( share/${PROJECT_NAME}/ ) +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + set(_pytest_tests + test/test_xacro_processing.py + ) + foreach(_test_path ${_pytest_tests}) + get_filename_component(_test_name ${_test_path} NAME_WE) + ament_add_pytest_test(${_test_name} ${_test_path} + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + TIMEOUT 60 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + endforeach() +endif() + ament_package() diff --git a/andino_description/README.md b/andino_description/README.md index 73d299e2..8e59ab99 100644 --- a/andino_description/README.md +++ b/andino_description/README.md @@ -5,6 +5,14 @@ This package holds the urdf description of the robot. +In the `urdf` folder you have the URDF files that contain the description of the robot, divided in different modules and merged into `andino.urdf.xacro` file. + +## Configuration + +In case you want to change the physical properties of some of the components of the robot, you can do it modifying the default YAML files inside the `config/andino` folder. + +You can even add your own configuration files in another directory in the `config` folder, and pass this directory to the main file using the `yaml_config_dir` xacro argument on the launch files. + ## Launch Files For launching robot state publisher for filling up static tf information and serving the description of the robot. Typically used during robot bringup. @@ -16,5 +24,3 @@ For launching the robot state publisher and providing some visualization with rv ``` ros2 launch andino_description view_andino.launch.py ``` - - diff --git a/andino_description/config/andino/hardware.yaml b/andino_description/config/andino/hardware.yaml new file mode 100644 index 00000000..cc162915 --- /dev/null +++ b/andino_description/config/andino/hardware.yaml @@ -0,0 +1,6 @@ +left_wheel_name: left_wheel_joint +right_wheel_name: right_wheel_joint +serial_device: /dev/ttyUSB_ARDUINO +baud_rate: 57600 +timeout: 1000 +enc_ticks_per_rev: 585 diff --git a/andino_description/launch/andino_description.launch.py b/andino_description/launch/andino_description.launch.py index 78122fa2..396059d7 100644 --- a/andino_description/launch/andino_description.launch.py +++ b/andino_description/launch/andino_description.launch.py @@ -31,7 +31,7 @@ import os from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription +from launch import LaunchDescription, LaunchContext from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration @@ -49,7 +49,8 @@ def generate_launch_description(): pkg_andino_description = get_package_share_directory('andino_description') # Obtain urdf from xacro files. - doc = xacro.process_file(os.path.join(pkg_andino_description, 'urdf', 'andino.urdf.xacro')) + arguments = {'yaml_config_dir': os.path.join(pkg_andino_description, 'config', 'andino')} + doc = xacro.process_file(os.path.join(pkg_andino_description, 'urdf', 'andino.urdf.xacro'), mappings = arguments) robot_desc = doc.toprettyxml(indent=' ') params = {'robot_description': robot_desc, 'publish_frequency': 30.0} diff --git a/andino_description/launch/view_andino.launch.py b/andino_description/launch/view_andino.launch.py index 33f93169..396b8a16 100644 --- a/andino_description/launch/view_andino.launch.py +++ b/andino_description/launch/view_andino.launch.py @@ -53,7 +53,8 @@ def generate_launch_description(): pkg_andino_description = get_package_share_directory('andino_description') # Obtain urdf from xacro files. - doc = xacro.process_file(os.path.join(pkg_andino_description, 'urdf', 'andino.urdf.xacro')) + arguments = {'yaml_config_dir': os.path.join(pkg_andino_description, 'config', 'andino')} + doc = xacro.process_file(os.path.join(pkg_andino_description, 'urdf', 'andino.urdf.xacro'), mappings = arguments) robot_desc = doc.toprettyxml(indent=' ') params = {'robot_description': robot_desc, 'publish_frequency': 30.0} diff --git a/andino_description/package.xml b/andino_description/package.xml index 5d630556..20d4cac2 100644 --- a/andino_description/package.xml +++ b/andino_description/package.xml @@ -9,6 +9,7 @@ BSD Clause 3 ament_cmake + ament_cmake_python joint_state_publisher_gui robot_state_publisher @@ -16,6 +17,9 @@ rviz2 xacro + ament_cmake_pytest + ament_index_python + ament_cmake diff --git a/andino_description/test/test_xacro_processing.py b/andino_description/test/test_xacro_processing.py new file mode 100644 index 00000000..6b3fe7fc --- /dev/null +++ b/andino_description/test/test_xacro_processing.py @@ -0,0 +1,45 @@ +# BSD 3-Clause License + +# Copyright (c) 2024, Ekumen Inc. +# All rights reserved. + +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: + +# 1. Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. + +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. + +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. + +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import os +import pytest +import xacro +from ament_index_python.packages import get_package_share_directory + +def test_xacro_processing(): + """Test main xacro file (andino.urdf.xacro) processing""" + # Get the file path. + xacro_file_path = os.path.join(get_package_share_directory("andino_description"), 'urdf', 'andino.urdf.xacro') + + # Test xacro processing. + try: + xacro.process_file(xacro_file_path) + except Exception as e: + pytest.fail(f"Xacro processing failed: {e}") diff --git a/andino_description/urdf/andino.urdf.xacro b/andino_description/urdf/andino.urdf.xacro index 52377037..e34ee5d6 100644 --- a/andino_description/urdf/andino.urdf.xacro +++ b/andino_description/urdf/andino.urdf.xacro @@ -30,6 +30,10 @@ + + + + @@ -92,6 +96,7 @@ + diff --git a/andino_description/urdf/include/andino_control.urdf.xacro b/andino_description/urdf/include/andino_control.urdf.xacro index 9ccea3b6..533c2444 100644 --- a/andino_description/urdf/include/andino_control.urdf.xacro +++ b/andino_description/urdf/include/andino_control.urdf.xacro @@ -1,6 +1,5 @@ - @@ -43,5 +42,4 @@ - diff --git a/andino_firmware/README.md b/andino_firmware/README.md index c8995ae9..cb1e2d8e 100644 --- a/andino_firmware/README.md +++ b/andino_firmware/README.md @@ -8,8 +8,28 @@ Check `encoder_driver.h` and `motor_driver.h` files to check the expected pins f ## Installation +### Arduino +In Arduino IDE, go to `tools->Manage Libraries ...` and install: +- "Adafruit BNO055" + Verify and Upload `andino_firmware.ino` to your arduino board. +### PlatformIO +1. Install dependencies `sudo apt-get install python3.10-venv` +2. Install platformio +``` +curl -fsSL -o /tmp/get-platformio.py https://raw.githubusercontent.com/platformio/platformio-core-installer/master/get-platformio.py +python3 /tmp/get-platformio.py +``` +3. Add platformio to your $PATH: +``` +echo "PATH=\"\$PATH:\$HOME/.platformio/penv/bin\"" >> $HOME/.bashrc +source $HOME/.bashrc +``` +4. Build and upload the firmware + - If you're using an arduino uno `pio run --target upload -e uno` + - If you're using an arduino nano `pio run --target upload -e nanoatmega328` + ## Description Via `serial` connection (57600 baud) it is possible to interact with the microcontroller. The interface is described in the [commands.h](src/commands.h) file. Here are the most used commands: diff --git a/andino_firmware/platformio.ini b/andino_firmware/platformio.ini index b6cbfd70..6283cef5 100644 --- a/andino_firmware/platformio.ini +++ b/andino_firmware/platformio.ini @@ -8,23 +8,48 @@ ; Please visit documentation for the other options and examples ; https://docs.platformio.org/page/projectconf.html +[platformio] +default_envs = uno, nanoatmega328 + +; Base configuration for build tools. +[base_build] +build_flags = + -Wall -Wextra + ; Base configuration for Atmel AVR based Arduino boards. [base_atmelavr] platform = atmelavr framework = arduino monitor_speed = 57600 +test_ignore = desktop/* +lib_deps = + Wire + SPI + adafruit/Adafruit BNO055 + adafruit/Adafruit BusIO + adafruit/Adafruit Unified Sensor -; Base configuration for build tools. -[base_build] -build_flags = - -Wall -Wextra +; Base configuration for desktop platforms (for unit testing). +[base_desktop] +platform = native +test_framework = googletest +test_build_src = true +build_src_filter = + + + + + + + + ; Environment for Arduino Uno. [env:uno] -extends = base_atmelavr, base_build +extends = base_build, base_atmelavr board = uno ; Environment for Arduino Nano. [env:nanoatmega328] -extends = base_atmelavr, base_build +extends = base_build, base_atmelavr board = nanoatmega328 + +; Environment for desktop platforms (Windows, macOS, Linux, etc). +[env:desktop] +extends = base_build, base_desktop diff --git a/andino_firmware/src/app.cpp b/andino_firmware/src/app.cpp index faf41ef7..32c45ce7 100644 --- a/andino_firmware/src/app.cpp +++ b/andino_firmware/src/app.cpp @@ -64,125 +64,116 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "app.h" -#include "Arduino.h" +#include +#include +#include +#include +#include + #include "commands.h" #include "constants.h" +#include "digital_out_arduino.h" #include "encoder.h" #include "hw.h" +#include "interrupt_in_arduino.h" #include "motor.h" #include "pid.h" +#include "pwm_out_arduino.h" +#include "serial_stream_arduino.h" +#include "shell.h" -// TODO(jballoffet): Move this variables to a different module. +namespace andino { -/* Track the next time we make a PID calculation */ -unsigned long nextPID = andino::Constants::kPidPeriod; +SerialStreamArduino App::serial_stream_; -long lastMotorCommand = andino::Constants::kAutoStopWindow; +Shell App::shell_; -// A pair of varibles to help parse serial commands -int arg = 0; -int index = 0; +DigitalOutArduino App::left_motor_enable_digital_out_(Hw::kLeftMotorEnableGpioPin); +PwmOutArduino App::left_motor_forward_pwm_out_(Hw::kLeftMotorForwardGpioPin); +PwmOutArduino App::left_motor_backward_pwm_out_(Hw::kLeftMotorBackwardGpioPin); +Motor App::left_motor_(&left_motor_enable_digital_out_, &left_motor_forward_pwm_out_, + &left_motor_backward_pwm_out_); -// Variable to hold an input character -char chr; +DigitalOutArduino App::right_motor_enable_digital_out_(Hw::kRightMotorEnableGpioPin); +PwmOutArduino App::right_motor_forward_pwm_out_(Hw::kRightMotorForwardGpioPin); +PwmOutArduino App::right_motor_backward_pwm_out_(Hw::kRightMotorBackwardGpioPin); +Motor App::right_motor_(&right_motor_enable_digital_out_, &right_motor_forward_pwm_out_, + &right_motor_backward_pwm_out_); -// Variable to hold the current single-character command -char cmd; +InterruptInArduino App::left_encoder_channel_a_interrupt_in_(Hw::kLeftEncoderChannelAGpioPin); +InterruptInArduino App::left_encoder_channel_b_interrupt_in_(Hw::kLeftEncoderChannelBGpioPin); +Encoder App::left_encoder_(&left_encoder_channel_a_interrupt_in_, + &left_encoder_channel_b_interrupt_in_); -// Character arrays to hold the first and second arguments -char argv1[16]; -char argv2[16]; +InterruptInArduino App::right_encoder_channel_a_interrupt_in_(Hw::kRightEncoderChannelAGpioPin); +InterruptInArduino App::right_encoder_channel_b_interrupt_in_(Hw::kRightEncoderChannelBGpioPin); +Encoder App::right_encoder_(&right_encoder_channel_a_interrupt_in_, + &right_encoder_channel_b_interrupt_in_); -namespace andino { +Pid App::left_pid_controller_(Constants::kPidKp, Constants::kPidKd, Constants::kPidKi, + Constants::kPidKo, -Constants::kPwmMax, Constants::kPwmMax); +Pid App::right_pid_controller_(Constants::kPidKp, Constants::kPidKd, Constants::kPidKi, + Constants::kPidKo, -Constants::kPwmMax, Constants::kPwmMax); -Motor App::left_motor_(Hw::kLeftMotorEnableGpioPin, Hw::kLeftMotorForwardGpioPin, - Hw::kLeftMotorBackwardGpioPin); -Motor App::right_motor_(Hw::kRightMotorEnableGpioPin, Hw::kRightMotorForwardGpioPin, - Hw::kRightMotorBackwardGpioPin); +unsigned long App::last_pid_computation_{0}; -Encoder App::left_encoder_(Hw::kLeftEncoderChannelAGpioPin, Hw::kLeftEncoderChannelBGpioPin); -Encoder App::right_encoder_(Hw::kRightEncoderChannelAGpioPin, Hw::kRightEncoderChannelBGpioPin); +unsigned long App::last_set_motors_speed_cmd_{0}; -PID App::left_pid_controller_(Constants::kPidKp, Constants::kPidKd, Constants::kPidKi, - Constants::kPidKo, -Constants::kPwmMax, Constants::kPwmMax); -PID App::right_pid_controller_(Constants::kPidKp, Constants::kPidKd, Constants::kPidKi, - Constants::kPidKo, -Constants::kPwmMax, Constants::kPwmMax); +bool App::is_imu_connected{false}; + +Adafruit_BNO055 App::bno055_imu_{/*sensorID=*/55, BNO055_ADDRESS_A, &Wire}; void App::setup() { // Required by Arduino libraries to work. init(); - Serial.begin(Constants::kBaudrate); + serial_stream_.begin(Constants::kBaudrate); - left_encoder_.init(); - right_encoder_.init(); + left_encoder_.begin(); + right_encoder_.begin(); - // Enable motors. - left_motor_.set_state(true); - right_motor_.set_state(true); + left_motor_.begin(); + left_motor_.enable(true); + right_motor_.begin(); + right_motor_.enable(true); left_pid_controller_.reset(left_encoder_.read()); right_pid_controller_.reset(right_encoder_.read()); + + // Initialize command shell. + shell_.set_serial_stream(&serial_stream_); + shell_.set_default_callback(cmd_unknown_cb); + shell_.register_command(Commands::kReadAnalogGpio, cmd_read_analog_gpio_cb); + shell_.register_command(Commands::kReadDigitalGpio, cmd_read_digital_gpio_cb); + shell_.register_command(Commands::kReadEncoders, cmd_read_encoders_cb); + shell_.register_command(Commands::kResetEncoders, cmd_reset_encoders_cb); + shell_.register_command(Commands::kSetMotorsSpeed, cmd_set_motors_speed_cb); + shell_.register_command(Commands::kSetMotorsPwm, cmd_set_motors_pwm_cb); + shell_.register_command(Commands::kSetPidsTuningGains, cmd_set_pid_tuning_gains_cb); + shell_.register_command(Commands::kGetIsImuConnected, cmd_get_is_imu_connected_cb); + shell_.register_command(Commands::kReadEncodersAndImu, cmd_read_encoders_and_imu_cb); + + // Initialize IMU sensor. + if (bno055_imu_.begin()) { + bno055_imu_.setExtCrystalUse(true); + is_imu_connected = true; + } } void App::loop() { - while (Serial.available() > 0) { - // Read the next character - chr = Serial.read(); - - // Terminate a command with a CR - if (chr == 13) { - if (arg == 1) - argv1[index] = 0; - else if (arg == 2) - argv2[index] = 0; - run_command(); - reset_command(); - } - // Use spaces to delimit parts of the command - else if (chr == ' ') { - // Step through the arguments - if (arg == 0) - arg = 1; - else if (arg == 1) { - argv1[index] = 0; - arg = 2; - index = 0; - } - continue; - } else { - if (arg == 0) { - // The first arg is the single-letter command - cmd = chr; - } else if (arg == 1) { - // Subsequent arguments can be more than one character - argv1[index] = chr; - index++; - } else if (arg == 2) { - argv2[index] = chr; - index++; - } - } - } + // Process command prompt input. + shell_.process_input(); - // Run a PID calculation at the appropriate intervals - if (millis() > nextPID) { - int left_motor_speed = 0; - int right_motor_speed = 0; - left_pid_controller_.compute(left_encoder_.read(), left_motor_speed); - right_pid_controller_.compute(right_encoder_.read(), right_motor_speed); - left_motor_.set_speed(left_motor_speed); - right_motor_.set_speed(right_motor_speed); - nextPID += Constants::kPidPeriod; + // Compute PID output at the configured rate. + if ((millis() - last_pid_computation_) > Constants::kPidPeriod) { + last_pid_computation_ = millis(); + adjust_motors_speed(); } - // Check to see if we have exceeded the auto-stop interval - if ((millis() - lastMotorCommand) > Constants::kAutoStopWindow) { - lastMotorCommand = millis(); - left_motor_.set_speed(0); - right_motor_.set_speed(0); - left_pid_controller_.enable(false); - right_pid_controller_.enable(false); + // Stop the motors if auto-stop interval has been reached. + if ((millis() - last_set_motors_speed_cmd_) > Constants::kAutoStopWindow) { + last_set_motors_speed_cmd_ = millis(); + stop_motors(); } // Required by Arduino libraries to work. @@ -191,62 +182,53 @@ void App::loop() { } } -void App::reset_command() { - cmd = 0; - memset(argv1, 0, sizeof(argv1)); - memset(argv2, 0, sizeof(argv2)); - arg = 0; - index = 0; +void App::adjust_motors_speed() { + int left_motor_speed = 0; + int right_motor_speed = 0; + left_pid_controller_.compute(left_encoder_.read(), left_motor_speed); + right_pid_controller_.compute(right_encoder_.read(), right_motor_speed); + if (left_pid_controller_.enabled()) { + left_motor_.set_speed(left_motor_speed); + } + if (right_pid_controller_.enabled()) { + right_motor_.set_speed(right_motor_speed); + } } -void App::run_command() { - switch (cmd) { - case Commands::kReadAnalogGpio: - cmd_read_analog_gpio(argv1, argv2); - break; - case Commands::kReadDigitalGpio: - cmd_read_digital_gpio(argv1, argv2); - break; - case Commands::kReadEncoders: - cmd_read_encoders(argv1, argv2); - break; - case Commands::kResetEncoders: - cmd_reset_encoders(argv1, argv2); - break; - case Commands::kSetMotorsSpeed: - cmd_set_motors_speed(argv1, argv2); - break; - case Commands::kSetMotorsPwm: - cmd_set_motors_pwm(argv1, argv2); - break; - case Commands::kSetPidsTuningGains: - cmd_set_pid_tuning_gains(argv1, argv2); - break; - default: - cmd_unknown(argv1, argv2); - break; - } +void App::stop_motors() { + left_motor_.set_speed(0); + right_motor_.set_speed(0); + left_pid_controller_.disable(); + right_pid_controller_.disable(); } -void App::cmd_unknown(const char*, const char*) { Serial.println("Unknown command."); } +void App::cmd_unknown_cb(int, char**) { Serial.println("Unknown command."); } + +void App::cmd_read_analog_gpio_cb(int argc, char** argv) { + if (argc < 2) { + return; + } -void App::cmd_read_analog_gpio(const char* arg1, const char*) { - const int pin = atoi(arg1); + const int pin = atoi(argv[1]); Serial.println(analogRead(pin)); } -void App::cmd_read_digital_gpio(const char* arg1, const char*) { - const int pin = atoi(arg1); +void App::cmd_read_digital_gpio_cb(int argc, char** argv) { + if (argc < 2) { + return; + } + + const int pin = atoi(argv[1]); Serial.println(digitalRead(pin)); } -void App::cmd_read_encoders(const char*, const char*) { +void App::cmd_read_encoders_cb(int, char**) { Serial.print(left_encoder_.read()); Serial.print(" "); Serial.println(right_encoder_.read()); } -void App::cmd_reset_encoders(const char*, const char*) { +void App::cmd_reset_encoders_cb(int, char**) { left_encoder_.reset(); right_encoder_.reset(); left_pid_controller_.reset(left_encoder_.read()); @@ -254,22 +236,26 @@ void App::cmd_reset_encoders(const char*, const char*) { Serial.println("OK"); } -void App::cmd_set_motors_speed(const char* arg1, const char* arg2) { - const int left_motor_speed = atoi(arg1); - const int right_motor_speed = atoi(arg2); +void App::cmd_set_motors_speed_cb(int argc, char** argv) { + if (argc < 3) { + return; + } + + const int left_motor_speed = atoi(argv[1]); + const int right_motor_speed = atoi(argv[2]); // Reset the auto stop timer. - lastMotorCommand = millis(); + last_set_motors_speed_cmd_ = millis(); if (left_motor_speed == 0 && right_motor_speed == 0) { left_motor_.set_speed(0); right_motor_.set_speed(0); left_pid_controller_.reset(left_encoder_.read()); right_pid_controller_.reset(right_encoder_.read()); - left_pid_controller_.enable(false); - right_pid_controller_.enable(false); + left_pid_controller_.disable(); + right_pid_controller_.disable(); } else { - left_pid_controller_.enable(true); - right_pid_controller_.enable(true); + left_pid_controller_.enable(); + right_pid_controller_.enable(); } // The target speeds are in ticks per second, so we need to convert them to ticks per @@ -279,23 +265,30 @@ void App::cmd_set_motors_speed(const char* arg1, const char* arg2) { Serial.println("OK"); } -void App::cmd_set_motors_pwm(const char* arg1, const char* arg2) { - const int left_motor_pwm = atoi(arg1); - const int right_motor_pwm = atoi(arg2); +void App::cmd_set_motors_pwm_cb(int argc, char** argv) { + if (argc < 3) { + return; + } + + const int left_motor_pwm = atoi(argv[1]); + const int right_motor_pwm = atoi(argv[2]); - // Reset the auto stop timer. - lastMotorCommand = millis(); left_pid_controller_.reset(left_encoder_.read()); right_pid_controller_.reset(right_encoder_.read()); // Sneaky way to temporarily disable the PID. - left_pid_controller_.enable(false); - right_pid_controller_.enable(false); + left_pid_controller_.disable(); + right_pid_controller_.disable(); left_motor_.set_speed(left_motor_pwm); right_motor_.set_speed(right_motor_pwm); Serial.println("OK"); } -void App::cmd_set_pid_tuning_gains(const char* arg1, const char*) { +void App::cmd_set_pid_tuning_gains_cb(int argc, char** argv) { + // TODO(jballoffet): Refactor to expect command multiple arguments. + if (argc < 2) { + return; + } + static constexpr int kSizePidArgs{4}; int i = 0; char arg[20]; @@ -303,7 +296,7 @@ void App::cmd_set_pid_tuning_gains(const char* arg1, const char*) { int pid_args[kSizePidArgs]{0, 0, 0, 0}; // Example: "u 30:20:10:50". - strcpy(arg, arg1); + strcpy(arg, argv[1]); char* p = arg; while ((str = strtok_r(p, ":", &p)) != NULL && i < kSizePidArgs) { pid_args[i] = atoi(str); @@ -322,4 +315,47 @@ void App::cmd_set_pid_tuning_gains(const char* arg1, const char*) { Serial.println("OK"); } +void App::cmd_get_is_imu_connected_cb(int, char**) { Serial.println(is_imu_connected); } + +void App::cmd_read_encoders_and_imu_cb(int, char**) { + Serial.print(left_encoder_.read()); + Serial.print(" "); + Serial.print(right_encoder_.read()); + Serial.print(" "); + + // Retrieve absolute orientation (quaternion). See + // https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor/overview for further + // information. + imu::Quaternion orientation = bno055_imu_.getQuat(); + Serial.print(orientation.x(), 4); + Serial.print(" "); + Serial.print(orientation.y(), 4); + Serial.print(" "); + Serial.print(orientation.z(), 4); + Serial.print(" "); + Serial.print(orientation.w(), 4); + Serial.print(" "); + + // Retrieve angular velocity (rad/s). See + // https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor/overview for further + // information. + imu::Vector<3> angular_velocity = bno055_imu_.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); + Serial.print(angular_velocity.x()); + Serial.print(" "); + Serial.print(angular_velocity.y()); + Serial.print(" "); + Serial.print(angular_velocity.z()); + Serial.print(" "); + + // Retrieve linear acceleration (m/s^2). See + // https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor/overview for further + // information. + imu::Vector<3> linear_acceleration = bno055_imu_.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); + Serial.print(linear_acceleration.x()); + Serial.print(" "); + Serial.print(linear_acceleration.y()); + Serial.print(" "); + Serial.print(linear_acceleration.z()); +} + } // namespace andino diff --git a/andino_firmware/src/app.h b/andino_firmware/src/app.h index b0745f6e..0bcbef8f 100644 --- a/andino_firmware/src/app.h +++ b/andino_firmware/src/app.h @@ -29,9 +29,16 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include + +#include "digital_out_arduino.h" #include "encoder.h" +#include "interrupt_in_arduino.h" #include "motor.h" #include "pid.h" +#include "pwm_out_arduino.h" +#include "serial_stream_arduino.h" +#include "shell.h" namespace andino { @@ -48,57 +55,85 @@ class App { static void loop(); private: - /// Clears the current command parameters. - // TODO(jballoffet): Move this method to a different module. - static void reset_command(); + /// Computes the PID output and updates the motors speed accordingly. + static void adjust_motors_speed(); - /// Runs a command. - // TODO(jballoffet): Move this method to a different module. - static void run_command(); + /// Stops the motors and disables the PID. + static void stop_motors(); /// Callback method for an unknown command (default). - // TODO(jballoffet): Parse arguments within callback method. - static void cmd_unknown(const char* arg1, const char* arg2); + static void cmd_unknown_cb(int argc, char** argv); /// Callback method for the `Commands::kReadAnalogGpio` command. - // TODO(jballoffet): Parse arguments within callback method. - static void cmd_read_analog_gpio(const char* arg1, const char* arg2); + static void cmd_read_analog_gpio_cb(int argc, char** argv); /// Callback method for the `Commands::kReadDigitalGpio` command. - // TODO(jballoffet): Parse arguments within callback method. - static void cmd_read_digital_gpio(const char* arg1, const char* arg2); + static void cmd_read_digital_gpio_cb(int argc, char** argv); /// Callback method for the `Commands::kReadEncoders` command. - // TODO(jballoffet): Parse arguments within callback method. - static void cmd_read_encoders(const char* arg1, const char* arg2); + static void cmd_read_encoders_cb(int argc, char** argv); /// Callback method for the `Commands::kResetEncoders` command. - // TODO(jballoffet): Parse arguments within callback method. - static void cmd_reset_encoders(const char* arg1, const char* arg2); + static void cmd_reset_encoders_cb(int argc, char** argv); /// Callback method for the `Commands::kSetMotorsSpeed` command. - // TODO(jballoffet): Parse arguments within callback method. - static void cmd_set_motors_speed(const char* arg1, const char* arg2); + static void cmd_set_motors_speed_cb(int argc, char** argv); /// Callback method for the `Commands::kSetMotorsPwm` command. - // TODO(jballoffet): Parse arguments within callback method. - static void cmd_set_motors_pwm(const char* arg1, const char* arg2); + static void cmd_set_motors_pwm_cb(int argc, char** argv); /// Callback method for the `Commands::kSetPidsTuningGains` command. - // TODO(jballoffet): Parse arguments within callback method. - static void cmd_set_pid_tuning_gains(const char* arg1, const char* arg2); + static void cmd_set_pid_tuning_gains_cb(int argc, char** argv); + + /// Callback method for the `Commands::kGetIsImuConnected` command. + static void cmd_get_is_imu_connected_cb(int argc, char** argv); + + /// Callback method for the `Commands::kReadEncodersAndImu` command. + static void cmd_read_encoders_and_imu_cb(int argc, char** argv); - /// Motors (one per wheel). + /// Serial stream. + static SerialStreamArduino serial_stream_; + + /// Application command shell. + static Shell shell_; + + /// Left wheel motor. + static DigitalOutArduino left_motor_enable_digital_out_; + static PwmOutArduino left_motor_forward_pwm_out_; + static PwmOutArduino left_motor_backward_pwm_out_; static Motor left_motor_; + + /// Right wheel motor. + static DigitalOutArduino right_motor_enable_digital_out_; + static PwmOutArduino right_motor_forward_pwm_out_; + static PwmOutArduino right_motor_backward_pwm_out_; static Motor right_motor_; - /// Encoders (one per wheel). + /// Left wheel encoder. + static InterruptInArduino left_encoder_channel_a_interrupt_in_; + static InterruptInArduino left_encoder_channel_b_interrupt_in_; static Encoder left_encoder_; + + /// Right wheel encoder. + static InterruptInArduino right_encoder_channel_a_interrupt_in_; + static InterruptInArduino right_encoder_channel_b_interrupt_in_; static Encoder right_encoder_; /// PID controllers (one per wheel). - static PID left_pid_controller_; - static PID right_pid_controller_; + static Pid left_pid_controller_; + static Pid right_pid_controller_; + + /// Adafruit BNO055 IMU sensor. + static Adafruit_BNO055 bno055_imu_; + + /// Tracks the last time the PID computation was made. + static unsigned long last_pid_computation_; + + /// Tracks the last time a `Commands::kSetMotorsSpeed` command was received. + static unsigned long last_set_motors_speed_cmd_; + + /// Tracks whether there is an IMU sensor connected. + static bool is_imu_connected; }; } // namespace andino diff --git a/andino_firmware/src/commands.h b/andino_firmware/src/commands.h index 43efa111..8136f2e3 100644 --- a/andino_firmware/src/commands.h +++ b/andino_firmware/src/commands.h @@ -69,19 +69,23 @@ namespace andino { /// @brief CLI commands. struct Commands { /// @brief Reads an analog GPIO. - static constexpr char kReadAnalogGpio{'a'}; + static constexpr const char* kReadAnalogGpio{"a"}; /// @brief Reads a digital GPIO. - static constexpr char kReadDigitalGpio{'d'}; + static constexpr const char* kReadDigitalGpio{"d"}; /// @brief Reads the encoders tick count values. - static constexpr char kReadEncoders{'e'}; + static constexpr const char* kReadEncoders{"e"}; /// @brief Sets the encoders ticks count to zero. - static constexpr char kResetEncoders{'r'}; + static constexpr const char* kResetEncoders{"r"}; /// @brief Sets the motors speed [ticks/s]. - static constexpr char kSetMotorsSpeed{'m'}; + static constexpr const char* kSetMotorsSpeed{"m"}; /// @brief Sets the motors PWM value [duty range: 0-255]. - static constexpr char kSetMotorsPwm{'o'}; + static constexpr const char* kSetMotorsPwm{"o"}; /// @brief Sets the PIDs tuning gains [format: "kp:kd:ki:ko"]. - static constexpr char kSetPidsTuningGains{'u'}; + static constexpr const char* kSetPidsTuningGains{"u"}; + /// @brief Gets whether there is an IMU sensor connected. + static constexpr const char* kGetIsImuConnected{"h"}; + /// @brief Reads the encoders tick count values and IMU sensor data. + static constexpr const char* kReadEncodersAndImu{"i"}; }; } // namespace andino diff --git a/andino_firmware/src/digital_in.h b/andino_firmware/src/digital_in.h new file mode 100644 index 00000000..7f5bb8f2 --- /dev/null +++ b/andino_firmware/src/digital_in.h @@ -0,0 +1,58 @@ +// BSD 3-Clause License +// +// Copyright (c) 2024, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +namespace andino { + +/// @brief This class defines an interface for digital inputs. +class DigitalIn { + public: + /// @brief Constructs a DigitalIn using the specified GPIO pin. + /// + /// @param gpio_pin GPIO pin. + explicit DigitalIn(const int gpio_pin) : gpio_pin_(gpio_pin) {} + + /// @brief Destructs the digital input. + virtual ~DigitalIn() = default; + + /// @brief Initializes the digital input. + virtual void begin() const = 0; + + /// @brief Gets the digital input value (0 or 1). + /// + /// @return Digital input value. + virtual int read() const = 0; + + protected: + /// GPIO pin. + const int gpio_pin_; +}; + +} // namespace andino diff --git a/andino_firmware/src/digital_out.h b/andino_firmware/src/digital_out.h new file mode 100644 index 00000000..6a5c35a2 --- /dev/null +++ b/andino_firmware/src/digital_out.h @@ -0,0 +1,58 @@ +// BSD 3-Clause License +// +// Copyright (c) 2024, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +namespace andino { + +/// @brief This class defines an interface for digital outputs. +class DigitalOut { + public: + /// @brief Constructs a DigitalOut using the specified GPIO pin. + /// + /// @param gpio_pin GPIO pin. + explicit DigitalOut(const int gpio_pin) : gpio_pin_(gpio_pin) {} + + /// @brief Destructs the digital output. + virtual ~DigitalOut() = default; + + /// @brief Initializes the digital output. + virtual void begin() const = 0; + + /// @brief Sets the digital output value (0 or 1). + /// + /// @param value Digital output value. + virtual void write(int value) const = 0; + + protected: + /// GPIO pin. + const int gpio_pin_; +}; + +} // namespace andino diff --git a/andino_firmware/src/digital_out_arduino.cpp b/andino_firmware/src/digital_out_arduino.cpp new file mode 100644 index 00000000..d6bf6299 --- /dev/null +++ b/andino_firmware/src/digital_out_arduino.cpp @@ -0,0 +1,40 @@ +// BSD 3-Clause License +// +// Copyright (c) 2024, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "digital_out_arduino.h" + +#include + +namespace andino { + +void DigitalOutArduino::begin() const { pinMode(gpio_pin_, OUTPUT); } + +void DigitalOutArduino::write(int value) const { digitalWrite(gpio_pin_, value); } + +} // namespace andino diff --git a/andino_firmware/src/digital_out_arduino.h b/andino_firmware/src/digital_out_arduino.h new file mode 100644 index 00000000..3e476dde --- /dev/null +++ b/andino_firmware/src/digital_out_arduino.h @@ -0,0 +1,49 @@ +// BSD 3-Clause License +// +// Copyright (c) 2024, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +#include "digital_out.h" + +namespace andino { + +/// @brief This class provides an Arduino implementation of the digital output interface. +class DigitalOutArduino : public DigitalOut { + public: + /// @brief Constructs a DigitalOutArduino using the specified GPIO pin. + /// + /// @param gpio_pin GPIO pin. + explicit DigitalOutArduino(const int gpio_pin) : DigitalOut(gpio_pin) {} + + void begin() const override; + + void write(int value) const override; +}; + +} // namespace andino diff --git a/andino_firmware/src/encoder.cpp b/andino_firmware/src/encoder.cpp index 313548c3..8ddd8a87 100644 --- a/andino_firmware/src/encoder.cpp +++ b/andino_firmware/src/encoder.cpp @@ -66,14 +66,13 @@ #include -#include "Arduino.h" -#include "pcint.h" +#include "interrupt_in.h" namespace andino { constexpr int8_t Encoder::kTicksDelta[]; -const PCInt::InterruptCallback Encoder::kCallbacks[kInstancesMax] = {callback_0, callback_1}; +const InterruptIn::InterruptCallback Encoder::kCallbacks[kInstancesMax] = {callback_0, callback_1}; void Encoder::callback_0() { if (Encoder::instances_[0] != nullptr) { @@ -90,17 +89,17 @@ void Encoder::callback_1() { Encoder* Encoder::instances_[kInstancesMax] = {nullptr, nullptr}; int Encoder::instance_count_ = 0; -void Encoder::init() { +void Encoder::begin() { // The current implementation only supports two instances of this class to be constructed. This // prevents reaching a buffer overflow. if (instance_count_ == kInstancesMax) { return; } - pinMode(a_gpio_pin_, INPUT_PULLUP); - pinMode(b_gpio_pin_, INPUT_PULLUP); - andino::PCInt::attach_interrupt(a_gpio_pin_, kCallbacks[instance_count_]); - andino::PCInt::attach_interrupt(b_gpio_pin_, kCallbacks[instance_count_]); + channel_a_interrupt_in_->begin(); + channel_a_interrupt_in_->attach(kCallbacks[instance_count_]); + channel_b_interrupt_in_->begin(); + channel_b_interrupt_in_->attach(kCallbacks[instance_count_]); instances_[instance_count_] = this; instance_count_++; @@ -113,7 +112,7 @@ void Encoder::reset() { count_ = 0L; } void Encoder::callback() { // Read the current channels state into the lowest 2 bits of the encoder state. state_ <<= 2; - state_ |= (digitalRead(b_gpio_pin_) << 1) | digitalRead(a_gpio_pin_); + state_ |= (channel_b_interrupt_in_->read() << 1) | channel_a_interrupt_in_->read(); // Update the encoder count accordingly. count_ += kTicksDelta[(state_ & 0x0F)]; diff --git a/andino_firmware/src/encoder.h b/andino_firmware/src/encoder.h index f2d3f0b3..e3c6b627 100644 --- a/andino_firmware/src/encoder.h +++ b/andino_firmware/src/encoder.h @@ -66,7 +66,7 @@ #include -#include "pcint.h" +#include "interrupt_in.h" namespace andino { @@ -77,14 +77,16 @@ class Encoder { public: /// @brief Constructs a new Encoder object. /// - /// @param a_gpio_pin Encoder channel A GPIO pin. - /// @param b_gpio_pin Encoder channel B GPIO pin. - Encoder(int a_gpio_pin, int b_gpio_pin) : a_gpio_pin_(a_gpio_pin), b_gpio_pin_(b_gpio_pin) {} + /// @param channel_a_interrupt_in Digital interrupt input connected to encoder channel A pin. + /// @param channel_b_interrupt_in Digital interrupt input connected to encoder channel B pin. + Encoder(const InterruptIn* channel_a_interrupt_in, const InterruptIn* channel_b_interrupt_in) + : channel_a_interrupt_in_(channel_a_interrupt_in), + channel_b_interrupt_in_(channel_b_interrupt_in) {} /// @brief Initializes the encoder. - void init(); + void begin(); - /// @brief Returns the ticks count value. + /// @brief Gets the ticks count value. /// /// @return Ticks count value. long read(); @@ -121,7 +123,7 @@ class Encoder { static constexpr int kInstancesMax{2}; /// Static wrappers that redirect to instance callback methods. - static const PCInt::InterruptCallback kCallbacks[kInstancesMax]; + static const InterruptIn::InterruptCallback kCallbacks[kInstancesMax]; /// Static wrapper that redirects to the first instance callback method. static void callback_0(); @@ -129,7 +131,7 @@ class Encoder { /// Static wrapper that redirects to the second instance callback method. static void callback_1(); - /// Channels GPIO interrupt callback. + /// Channels interrupt callback. void callback(); /// Holds references to the constructed Encoder instances. @@ -138,11 +140,11 @@ class Encoder { /// Number of constructed Encoder instances. static int instance_count_; - /// Channel A GPIO pin. - int a_gpio_pin_; + /// Digital interrupt input connected to encoder channel A pin. + const InterruptIn* channel_a_interrupt_in_; - /// Channel B GPIO pin. - int b_gpio_pin_; + /// Digital interrupt input connected to encoder channel B pin. + const InterruptIn* channel_b_interrupt_in_; /// Encoder state. It contains both the current and previous channels state readings: /// +------+-----+-----+-----+-----+-----+-----+-----+-----+ diff --git a/andino_firmware/src/hw.h b/andino_firmware/src/hw.h index 0b50b2d9..850525ac 100644 --- a/andino_firmware/src/hw.h +++ b/andino_firmware/src/hw.h @@ -38,10 +38,10 @@ struct Hw { /// @brief Left encoder channel B pin. Connected to PD3 (digital pin 3). static constexpr int kLeftEncoderChannelBGpioPin{3}; - /// @brief Right encoder channel A pin. Connected to PC4 (digital pin 18, analog pin A4). - static constexpr int kRightEncoderChannelAGpioPin{18}; - /// @brief Right encoder channel B pin. Connected to PC5 (digital pin 19, analog pin A5). - static constexpr int kRightEncoderChannelBGpioPin{19}; + /// @brief Right encoder channel A pin. Connected to PC2 (digital pin 16, analog pin A2). + static constexpr int kRightEncoderChannelAGpioPin{16}; + /// @brief Right encoder channel B pin. Connected to PC3 (digital pin 17, analog pin A3). + static constexpr int kRightEncoderChannelBGpioPin{17}; /// @brief Left motor driver backward pin. Connected to PD6 (digital pin 6). static constexpr int kLeftMotorBackwardGpioPin{6}; @@ -60,6 +60,11 @@ struct Hw { /// @note The enable input of the L298N motor driver may be directly jumped to 5V if the board has /// a jumper to do so. static constexpr int kRightMotorEnableGpioPin{12}; + + /// @brief IMU sensor I2C SCL pin. Connected to PC5 (digital pin 19, analog pin A5). + static constexpr int kImuI2cSclPin{19}; + /// @brief IMU sensor I2C SDA pin. Connected to PC4 (digital pin 18, analog pin A4). + static constexpr int kImuI2cSdaPin{18}; }; } // namespace andino diff --git a/andino_firmware/src/pcint.h b/andino_firmware/src/interrupt_in.h similarity index 78% rename from andino_firmware/src/pcint.h rename to andino_firmware/src/interrupt_in.h index 27e4f891..811466a6 100644 --- a/andino_firmware/src/pcint.h +++ b/andino_firmware/src/interrupt_in.h @@ -1,6 +1,6 @@ // BSD 3-Clause License // -// Copyright (c) 2023, Ekumen Inc. +// Copyright (c) 2024, Ekumen Inc. // All rights reserved. // // Redistribution and use in source and binary forms, with or without @@ -29,30 +29,28 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include - -#include "Arduino.h" +#include "digital_in.h" namespace andino { -/// @brief This class provides a simple way to set and use Pin Change Interrupts. -class PCInt { +/// @brief This class defines an interface for digital interrupt inputs. +class InterruptIn : public DigitalIn { public: /// @brief Interrupt callback type. typedef void (*InterruptCallback)(); - /// @brief Attaches an interrupt callback. - /// @note Only one callback per port is supported. + /// @brief Constructs a InterruptIn using the specified GPIO pin. /// - /// @param pin Pin of interest. - /// @param callback Callback function. - static void attach_interrupt(uint8_t pin, InterruptCallback callback); + /// @param gpio_pin GPIO pin. + explicit InterruptIn(const int gpio_pin) : DigitalIn(gpio_pin) {} - private: - PCInt() = delete; + /// @brief Destructs the digital interrupt input. + virtual ~InterruptIn() = default; - /// Map between ports and Pin Change Mask registers. - static constexpr volatile uint8_t* kPortToPCMask[]{&PCMSK0, &PCMSK1, &PCMSK2}; + /// @brief Attaches an interrupt callback. + /// + /// @param callback Callback function. + virtual void attach(InterruptCallback callback) const = 0; }; } // namespace andino diff --git a/andino_firmware/src/pcint.cpp b/andino_firmware/src/interrupt_in_arduino.cpp similarity index 73% rename from andino_firmware/src/pcint.cpp rename to andino_firmware/src/interrupt_in_arduino.cpp index ebc17b03..4e31e0d4 100644 --- a/andino_firmware/src/pcint.cpp +++ b/andino_firmware/src/interrupt_in_arduino.cpp @@ -1,6 +1,6 @@ // BSD 3-Clause License // -// Copyright (c) 2023, Ekumen Inc. +// Copyright (c) 2024, Ekumen Inc. // All rights reserved. // // Redistribution and use in source and binary forms, with or without @@ -27,24 +27,28 @@ // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -#include "pcint.h" +#include "interrupt_in_arduino.h" -#include "Arduino.h" +#include -static andino::PCInt::InterruptCallback g_callbacks[3] = {nullptr}; +/// Holds the attached callbacks. +static andino::InterruptIn::InterruptCallback g_callbacks[3] = {nullptr}; +/// Pin change interrupt request 0 interrupt service routine. ISR(PCINT0_vect) { if (g_callbacks[0] != nullptr) { g_callbacks[0](); } } +/// Pin change interrupt request 1 interrupt service routine. ISR(PCINT1_vect) { if (g_callbacks[1] != nullptr) { g_callbacks[1](); } } +/// Pin change interrupt request 2 interrupt service routine. ISR(PCINT2_vect) { if (g_callbacks[2] != nullptr) { g_callbacks[2](); @@ -53,11 +57,16 @@ ISR(PCINT2_vect) { namespace andino { -constexpr volatile uint8_t* PCInt::kPortToPCMask[]; +/// Map between ports and Pin Change Mask registers. +static constexpr volatile uint8_t* kPortToPCMask[]{&PCMSK0, &PCMSK1, &PCMSK2}; -void PCInt::attach_interrupt(uint8_t pin, InterruptCallback callback) { - uint8_t bit_mask = digitalPinToBitMask(pin); - uint8_t port = digitalPinToPort(pin); +void InterruptInArduino::begin() const { pinMode(gpio_pin_, INPUT_PULLUP); } + +int InterruptInArduino::read() const { return digitalRead(gpio_pin_); } + +void InterruptInArduino::attach(InterruptCallback callback) const { + uint8_t bit_mask = digitalPinToBitMask(gpio_pin_); + uint8_t port = digitalPinToPort(gpio_pin_); if (port == NOT_A_PORT) { return; diff --git a/andino_firmware/src/interrupt_in_arduino.h b/andino_firmware/src/interrupt_in_arduino.h new file mode 100644 index 00000000..37c6a225 --- /dev/null +++ b/andino_firmware/src/interrupt_in_arduino.h @@ -0,0 +1,51 @@ +// BSD 3-Clause License +// +// Copyright (c) 2024, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +#include "interrupt_in.h" + +namespace andino { + +/// @brief This class provides an Arduino implementation of the digital interrupt input interface. +class InterruptInArduino : public InterruptIn { + public: + /// @brief Constructs a InterruptInArduino using the specified GPIO pin. + /// + /// @param gpio_pin GPIO pin. + explicit InterruptInArduino(const int gpio_pin) : InterruptIn(gpio_pin) {} + + void begin() const override; + + int read() const override; + + void attach(InterruptCallback callback) const override; +}; + +} // namespace andino diff --git a/andino_firmware/src/motor.cpp b/andino_firmware/src/motor.cpp index 899756c6..3fe73c1d 100644 --- a/andino_firmware/src/motor.cpp +++ b/andino_firmware/src/motor.cpp @@ -64,18 +64,19 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "motor.h" -#include "Arduino.h" +#include "digital_out.h" +#include "pwm_out.h" namespace andino { -void Motor::set_state(bool enabled) { - if (enabled) { - digitalWrite(enable_gpio_pin_, HIGH); - } else { - digitalWrite(enable_gpio_pin_, LOW); - } +void Motor::begin() { + enable_digital_out_->begin(); + forward_pwm_out_->begin(); + backward_pwm_out_->begin(); } +void Motor::enable(bool enabled) { enable_digital_out_->write(enabled ? 1 : 0); } + void Motor::set_speed(int speed) { bool forward = true; @@ -89,11 +90,11 @@ void Motor::set_speed(int speed) { // The motor speed is controlled by sending a PWM wave to the corresponding pin. if (forward) { - analogWrite(forward_gpio_pin_, speed); - analogWrite(backward_gpio_pin_, 0); + forward_pwm_out_->write(speed); + backward_pwm_out_->write(0); } else { - analogWrite(backward_gpio_pin_, speed); - analogWrite(forward_gpio_pin_, 0); + backward_pwm_out_->write(speed); + forward_pwm_out_->write(0); } } diff --git a/andino_firmware/src/motor.h b/andino_firmware/src/motor.h index ae675163..df28f57d 100644 --- a/andino_firmware/src/motor.h +++ b/andino_firmware/src/motor.h @@ -64,6 +64,9 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include "digital_out.h" +#include "pwm_out.h" + namespace andino { /// @brief This class allows to control a DC motor by enabling it and setting its speed. The @@ -73,18 +76,22 @@ class Motor { public: /// @brief Constructs a new Motor object. /// - /// @param enable_gpio_pin Motor enable GPIO pin. - /// @param forward_gpio_pin Motor forward GPIO pin. - /// @param backward_gpio_pin Motor backward GPIO pin. - Motor(int enable_gpio_pin, int forward_gpio_pin, int backward_gpio_pin) - : enable_gpio_pin_(enable_gpio_pin), - forward_gpio_pin_(forward_gpio_pin), - backward_gpio_pin_(backward_gpio_pin) {} + /// @param enable_digital_out Digital output connected to motor enable pin. + /// @param forward_pwm_out PWM output connected to motor forward pin. + /// @param backward_pwm_out PWM output connected to motor backward pin. + Motor(const DigitalOut* enable_digital_out, const PwmOut* forward_pwm_out, + const PwmOut* backward_pwm_out) + : enable_digital_out_(enable_digital_out), + forward_pwm_out_(forward_pwm_out), + backward_pwm_out_(backward_pwm_out) {} + + /// @brief Initializes the motor. + void begin(); - /// @brief Sets the motor state. + /// @brief Enables the motor. /// - /// @param enabled Motor state. - void set_state(bool enabled); + /// @param enabled True to enable the motor, false otherwise. + void enable(bool enabled); /// @brief Sets the motor speed. /// @@ -98,14 +105,14 @@ class Motor { /// Maximum speed value. static constexpr int kMaxSpeed{255}; - /// Motor enable GPIO pin. - int enable_gpio_pin_; + /// Digital output connected to motor enable pin. + const DigitalOut* enable_digital_out_; - /// Motor forward GPIO pin. - int forward_gpio_pin_; + /// PWM output connected to motor forward pin. + const PwmOut* forward_pwm_out_; - /// Motor backward GPIO pin. - int backward_gpio_pin_; + /// PWM output connected to motor backward pin. + const PwmOut* backward_pwm_out_; }; } // namespace andino diff --git a/andino_firmware/src/pid.cpp b/andino_firmware/src/pid.cpp index 83cc78a4..476b8f40 100644 --- a/andino_firmware/src/pid.cpp +++ b/andino_firmware/src/pid.cpp @@ -71,7 +71,7 @@ namespace andino { // - http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/ // - http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ -void PID::reset(int encoder_count) { +void Pid::reset(int encoder_count) { // Since we can assume that the PID is only turned on when going from stop to moving, we can init // everything on zero. setpoint_ = 0; @@ -81,9 +81,16 @@ void PID::reset(int encoder_count) { last_output_ = 0; } -void PID::enable(bool enabled) { enabled_ = enabled; } +/// @brief Enable PID +void Pid::enable() { enabled_ = true; } -void PID::compute(int encoder_count, int& computed_output) { +/// @brief Is the PID controller enabled? +bool Pid::enabled() { return enabled_; } + +/// @brief Disable PID +void Pid::disable() { enabled_ = false; } + +void Pid::compute(int encoder_count, int& computed_output) { if (!enabled_) { // Reset PID once to prevent startup spikes. if (last_input_ != 0) { @@ -116,9 +123,9 @@ void PID::compute(int encoder_count, int& computed_output) { last_output_ = output; } -void PID::set_setpoint(int setpoint) { setpoint_ = setpoint; } +void Pid::set_setpoint(int setpoint) { setpoint_ = setpoint; } -void PID::set_tunings(int kp, int kd, int ki, int ko) { +void Pid::set_tunings(int kp, int kd, int ki, int ko) { kp_ = kp; kd_ = kd; ki_ = ki; diff --git a/andino_firmware/src/pid.h b/andino_firmware/src/pid.h index 768f8cb7..1bc4ca5a 100644 --- a/andino_firmware/src/pid.h +++ b/andino_firmware/src/pid.h @@ -32,7 +32,7 @@ namespace andino { /// @brief This class provides a simple PID controller implementation. -class PID { +class Pid { public: /// @brief Constructs a new PID object. /// @@ -42,23 +42,22 @@ class PID { /// @param ko Tuning output gain. /// @param output_min Output minimum limit. /// @param output_max Output maximum limit. - PID(int kp, int kd, int ki, int ko, int output_min, int output_max) - : kp_(kp), - kd_(kd), - ki_(ki), - ko_(ko), - output_min_(output_min), - output_max_(output_max) {} + Pid(int kp, int kd, int ki, int ko, int output_min, int output_max) + : kp_(kp), kd_(kd), ki_(ki), ko_(ko), output_min_(output_min), output_max_(output_max) {} /// @brief Resets the PID controller. /// /// @param encoder_count Current encoder value. void reset(int encoder_count); + /// @brief Returns if the PID controller is enabled or not. + bool enabled(); + /// @brief Enables the PID controller. - /// - /// @param enabled True to enable the PID, false otherwise. - void enable(bool enabled); + void enable(); + + /// @brief Disables the PID controller. + void disable(); /// @brief Computes a new output. /// diff --git a/andino_firmware/src/pwm_out.h b/andino_firmware/src/pwm_out.h new file mode 100644 index 00000000..65ec34ab --- /dev/null +++ b/andino_firmware/src/pwm_out.h @@ -0,0 +1,59 @@ +// BSD 3-Clause License +// +// Copyright (c) 2024, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +namespace andino { + +/// @brief This class defines an interface for PWM outputs. +class PwmOut { + public: + /// @brief Constructs a PwmOut using the specified GPIO pin. + /// + /// @param gpio_pin GPIO pin. + explicit PwmOut(const int gpio_pin) : gpio_pin_(gpio_pin) {} + + /// @brief Destructs the PWM output. + virtual ~PwmOut() {} + + /// @brief Initializes the PWM output. + virtual void begin() const = 0; + + /// @brief Sets the PWM output value (0 to 255). + /// + /// @param value PWM value. + // TODO(jballoffet): Change this API to expect values from 0 to 100 (%). + virtual void write(int value) const = 0; + + protected: + /// GPIO pin. + const int gpio_pin_; +}; + +} // namespace andino diff --git a/andino_firmware/src/pwm_out_arduino.cpp b/andino_firmware/src/pwm_out_arduino.cpp new file mode 100644 index 00000000..8bfc3584 --- /dev/null +++ b/andino_firmware/src/pwm_out_arduino.cpp @@ -0,0 +1,40 @@ +// BSD 3-Clause License +// +// Copyright (c) 2024, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "pwm_out_arduino.h" + +#include + +namespace andino { + +void PwmOutArduino::begin() const { pinMode(gpio_pin_, OUTPUT); } + +void PwmOutArduino::write(int value) const { analogWrite(gpio_pin_, value); } + +} // namespace andino diff --git a/andino_firmware/src/pwm_out_arduino.h b/andino_firmware/src/pwm_out_arduino.h new file mode 100644 index 00000000..77454131 --- /dev/null +++ b/andino_firmware/src/pwm_out_arduino.h @@ -0,0 +1,49 @@ +// BSD 3-Clause License +// +// Copyright (c) 2024, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +#include "pwm_out.h" + +namespace andino { + +/// @brief This class provides an Arduino implementation of the PWM output interface. +class PwmOutArduino : public PwmOut { + public: + /// @brief Constructs a PwmOutArduino using the specified GPIO pin. + /// + /// @param gpio_pin GPIO pin. + explicit PwmOutArduino(const int gpio_pin) : PwmOut(gpio_pin) {} + + void begin() const override; + + void write(int value) const override; +}; + +} // namespace andino diff --git a/andino_firmware/src/serial_stream.h b/andino_firmware/src/serial_stream.h new file mode 100644 index 00000000..4f58c899 --- /dev/null +++ b/andino_firmware/src/serial_stream.h @@ -0,0 +1,177 @@ +// BSD 3-Clause License +// +// Copyright (c) 2024, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +#include + +namespace andino { + +/// @brief This class defines an interface for serial streams. +class SerialStream { + public: + /// @brief Supported numeral systems. + enum Base { + kBin = 2, + kOct = 8, + kDec = 10, + kHex = 16, + }; + + /// @brief Constructs a SerialStream. + explicit SerialStream() = default; + + /// @brief Destructs the serial stream. + virtual ~SerialStream() = default; + + /// @brief Initializes the serial stream. + /// + /// @param baud Data rate in bits per second (baud). + virtual void begin(unsigned long baud) const = 0; + + /// @brief Gets the number of bytes available for reading. + /// + /// @return Number of bytes available for reading. + virtual int available() const = 0; + + /// @brief Gets the incoming data. + /// + /// @return First byte of incoming data, or -1 if no data is available. + virtual int read() const = 0; + + /// @brief Sends data as human-readable ASCII text. + /// + /// @param c String to send. + /// @return Number of bytes sent. + virtual size_t print(const char* c) const = 0; + + /// @brief Sends data as human-readable ASCII text. + /// + /// @param c Character to send. + /// @return Number of bytes sent. + virtual size_t print(char c) const = 0; + + /// @brief Sends data as human-readable ASCII text. + /// + /// @param b Byte to send. + /// @param base Numeral system to use for the representation. + /// @return Number of bytes sent. + virtual size_t print(unsigned char b, int base = kDec) const = 0; + + /// @brief Sends data as human-readable ASCII text. + /// + /// @param num Number to send. + /// @param base Numeral system to use for the representation. + /// @return Number of bytes sent. + virtual size_t print(int num, int base = kDec) const = 0; + + /// @brief Sends data as human-readable ASCII text. + /// + /// @param num Number to send. + /// @param base Numeral system to use for the representation. + /// @return Number of bytes sent. + virtual size_t print(unsigned int num, int base = kDec) const = 0; + + /// @brief Sends data as human-readable ASCII text. + /// + /// @param num Number to send. + /// @param base Numeral system to use for the representation. + /// @return Number of bytes sent. + virtual size_t print(long num, int base = kDec) const = 0; + + /// @brief Sends data as human-readable ASCII text. + /// + /// @param num Number to send. + /// @param base Numeral system to use for the representation. + /// @return Number of bytes sent. + virtual size_t print(unsigned long num, int base = kDec) const = 0; + + /// @brief Sends data as human-readable ASCII text. + /// + /// @param num Number to send. + /// @param digits Number of decimal places to use. + /// @return Number of bytes sent. + virtual size_t print(double num, int digits = 2) const = 0; + + /// @brief Sends data as human-readable ASCII text. + /// + /// @param c String to send. + /// @return Number of bytes sent. + virtual size_t println(const char* c) const = 0; + + /// @brief Sends data as human-readable ASCII text. + /// + /// @param c Character to send. + /// @return Number of bytes sent. + virtual size_t println(char c) const = 0; + + /// @brief Sends data as human-readable ASCII text. + /// + /// @param b Byte to send. + /// @param base Numeral system to use for the representation. + /// @return Number of bytes sent. + virtual size_t println(unsigned char b, int base = kDec) const = 0; + + /// @brief Sends data as human-readable ASCII text. + /// + /// @param num Number to send. + /// @param base Numeral system to use for the representation. + /// @return Number of bytes sent. + virtual size_t println(int num, int base = kDec) const = 0; + + /// @brief Sends data as human-readable ASCII text. + /// + /// @param num Number to send. + /// @param base Numeral system to use for the representation. + /// @return Number of bytes sent. + virtual size_t println(unsigned int num, int base = kDec) const = 0; + + /// @brief Sends data as human-readable ASCII text. + /// + /// @param num Number to send. + /// @param base Numeral system to use for the representation. + /// @return Number of bytes sent. + virtual size_t println(long num, int base = kDec) const = 0; + + /// @brief Sends data as human-readable ASCII text. + /// + /// @param num Number to send. + /// @param base Numeral system to use for the representation. + /// @return Number of bytes sent. + virtual size_t println(unsigned long num, int base = kDec) const = 0; + + /// @brief Sends data as human-readable ASCII text. + /// + /// @param num Number to send. + /// @param digits Number of decimal places to use. + /// @return Number of bytes sent. + virtual size_t println(double num, int digits = 2) const = 0; +}; + +} // namespace andino diff --git a/andino_firmware/src/serial_stream_arduino.cpp b/andino_firmware/src/serial_stream_arduino.cpp new file mode 100644 index 00000000..1e32be3f --- /dev/null +++ b/andino_firmware/src/serial_stream_arduino.cpp @@ -0,0 +1,90 @@ +// BSD 3-Clause License +// +// Copyright (c) 2024, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "serial_stream_arduino.h" + +#include + +#include + +namespace andino { + +void SerialStreamArduino::begin(unsigned long baud) const { Serial.begin(baud); } + +int SerialStreamArduino::available() const { return Serial.available(); } + +int SerialStreamArduino::read() const { return Serial.read(); } + +size_t SerialStreamArduino::print(const char* c) const { return Serial.print(c); } + +size_t SerialStreamArduino::print(char c) const { return Serial.print(c); } + +size_t SerialStreamArduino::print(unsigned char b, int base) const { return Serial.print(b, base); } + +size_t SerialStreamArduino::print(int num, int base) const { return Serial.print(num, base); } + +size_t SerialStreamArduino::print(unsigned int num, int base) const { + return Serial.print(num, base); +} + +size_t SerialStreamArduino::print(long num, int base) const { return Serial.print(num, base); } + +size_t SerialStreamArduino::print(unsigned long num, int base) const { + return Serial.print(num, base); +} + +size_t SerialStreamArduino::print(double num, int digits) const { + return Serial.print(num, digits); +} + +size_t SerialStreamArduino::println(const char* c) const { return Serial.println(c); } + +size_t SerialStreamArduino::println(char c) const { return Serial.println(c); } + +size_t SerialStreamArduino::println(unsigned char b, int base) const { + return Serial.println(b, base); +} + +size_t SerialStreamArduino::println(int num, int base) const { return Serial.println(num, base); } + +size_t SerialStreamArduino::println(unsigned int num, int base) const { + return Serial.println(num, base); +} + +size_t SerialStreamArduino::println(long num, int base) const { return Serial.println(num, base); } + +size_t SerialStreamArduino::println(unsigned long num, int base) const { + return Serial.println(num, base); +} + +size_t SerialStreamArduino::println(double num, int digits) const { + return Serial.println(num, digits); +} + +} // namespace andino diff --git a/andino_firmware/src/serial_stream_arduino.h b/andino_firmware/src/serial_stream_arduino.h new file mode 100644 index 00000000..6d3a95e6 --- /dev/null +++ b/andino_firmware/src/serial_stream_arduino.h @@ -0,0 +1,83 @@ +// BSD 3-Clause License +// +// Copyright (c) 2024, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +#include + +#include "serial_stream.h" + +namespace andino { + +/// @brief This class provides an Arduino implementation of the serial stream interface. +class SerialStreamArduino : public SerialStream { + public: + /// @brief Constructs a SerialStreamArduino. + explicit SerialStreamArduino() : SerialStream() {} + + void begin(unsigned long baud) const override; + + int available() const override; + + int read() const override; + + size_t print(const char* c) const override; + + size_t print(char c) const override; + + size_t print(unsigned char b, int base) const override; + + size_t print(int num, int base) const override; + + size_t print(unsigned int num, int base) const override; + + size_t print(long num, int base) const override; + + size_t print(unsigned long num, int base) const override; + + size_t print(double num, int digits) const override; + + size_t println(const char* c) const override; + + size_t println(char c) const override; + + size_t println(unsigned char b, int base) const override; + + size_t println(int num, int base) const override; + + size_t println(unsigned int num, int base) const override; + + size_t println(long num, int base) const override; + + size_t println(unsigned long num, int base) const override; + + size_t println(double num, int digits) const override; +}; + +} // namespace andino diff --git a/andino_firmware/src/shell.cpp b/andino_firmware/src/shell.cpp new file mode 100644 index 00000000..61aea393 --- /dev/null +++ b/andino_firmware/src/shell.cpp @@ -0,0 +1,105 @@ +// BSD 3-Clause License +// +// Copyright (c) 2023, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "shell.h" + +#include + +namespace andino { + +void Shell::set_serial_stream(const SerialStream* serial_stream) { serial_stream_ = serial_stream; } + +void Shell::set_default_callback(CommandCallback callback) { default_callback_ = callback; } + +void Shell::register_command(const char* name, CommandCallback callback) { + if (commands_count_ >= kCommandsMax) { + return; + } + + Command command; + strcpy(command.name, name); + command.callback = callback; + commands_[commands_count_++] = command; +} + +void Shell::process_input() { + while (serial_stream_->available() > 0) { + const char input = serial_stream_->read(); + + switch (input) { + case '\r': + // Terminate command prompt message and parse it. + message_buffer_[message_index_++] = '\0'; + parse_message(); + // Reset message buffer. + message_index_ = 0; + break; + + case '\n': + // Ignore newline characters. + break; + + default: + message_buffer_[message_index_++] = input; + // Prevent buffer overflow. + if (message_index_ >= kCommandPromptLengthMax) { + message_index_ = 0; + } + break; + } + } +} + +void Shell::parse_message() { + char* argv[kCommandArgMax]; + int argc = 0; + + argv[argc] = strtok(message_buffer_, " "); + while (argv[argc] != NULL && argc < (kCommandArgMax - 1)) { + argv[++argc] = strtok(NULL, " "); + } + + execute_callback(argc, argv); +} + +void Shell::execute_callback(int argc, char** argv) { + for (size_t i = 0; i < commands_count_; i++) { + if (!strcmp(argv[0], commands_[i].name)) { + commands_[i].callback(argc, argv); + return; + } + } + + // Unknown command received, executing default callback. + if (default_callback_ != nullptr) { + default_callback_(argc, argv); + } +} + +} // namespace andino diff --git a/andino_firmware/src/shell.h b/andino_firmware/src/shell.h new file mode 100644 index 00000000..f9c5d45d --- /dev/null +++ b/andino_firmware/src/shell.h @@ -0,0 +1,110 @@ +// BSD 3-Clause License +// +// Copyright (c) 2023, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +#include + +#include "serial_stream.h" + +namespace andino { + +/// @brief This class interprets and processes the commands entered by the user. +class Shell { + public: + /// @brief Command callback type. + typedef void (*CommandCallback)(int argc, char** argv); + + /// @brief Sets the serial stream to use. + /// + /// @param serial_stream Serial stream. + void set_serial_stream(const SerialStream* serial_stream); + + /// @brief Sets the default callback for unknown commands. + /// + /// @param callback Callback function. + void set_default_callback(CommandCallback callback); + + /// @brief Adds a command registry entry to the shell. + /// + /// @param name Command name. + /// @param callback Callback function. + void register_command(const char* name, CommandCallback callback); + + /// @brief Processes the available input at the command prompt (if any). Meant to be called + /// continously. + void process_input(); + + private: + /// Maximum command name length. + static constexpr int kCommandNameLengthMax{8}; + + /// Command registry entry definition. + struct Command { + /// Command name. + char name[kCommandNameLengthMax]; + /// Callback function. + CommandCallback callback; + }; + + /// Maximum number of commands that can be registered. + static constexpr int kCommandsMax{16}; + + /// Maximum number of command arguments that can be processed. + static constexpr int kCommandArgMax{16}; + + /// Maximum command prompt message length. + static constexpr int kCommandPromptLengthMax{64}; + + /// Parses the command prompt message. + void parse_message(); + + /// Executes the corresponding command callback function. + void execute_callback(int argc, char** argv); + + /// Serial stream. + const SerialStream* serial_stream_{nullptr}; + + /// Default callback for unknown commands. + CommandCallback default_callback_{nullptr}; + + /// Command registry. + Command commands_[kCommandsMax]; + + /// Number of registered commands. + size_t commands_count_{0}; + + /// Command prompt message. + char message_buffer_[kCommandPromptLengthMax]; + + /// Command prompt message index. + int message_index_{0}; +}; + +} // namespace andino diff --git a/andino_firmware/test/desktop/test_encoder/encoder_test.cpp b/andino_firmware/test/desktop/test_encoder/encoder_test.cpp new file mode 100644 index 00000000..3ccfff23 --- /dev/null +++ b/andino_firmware/test/desktop/test_encoder/encoder_test.cpp @@ -0,0 +1,164 @@ +// BSD 3-Clause License +// +// Copyright (c) 2024, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "encoder.h" + +#include +#include + +#include "interrupt_in.h" + +namespace andino { +namespace test { +namespace { + +using ::testing::_; +using ::testing::Return; + +class MockInterruptIn : public andino::InterruptIn { + public: + MockInterruptIn(const int gpio_pin) : andino::InterruptIn(gpio_pin) {} + MOCK_METHOD(void, begin, (), (const, override)); + MOCK_METHOD(int, read, (), (const, override)); + MOCK_METHOD(void, attach, (andino::InterruptIn::InterruptCallback callback), (const, override)); +}; + +class EncoderTest : public testing::Test { + protected: + MockInterruptIn channel_a_interrupt_in_{0}; + MockInterruptIn channel_b_interrupt_in_{0}; + andino::Encoder encoder_{&channel_a_interrupt_in_, &channel_b_interrupt_in_}; +}; + +static andino::InterruptIn::InterruptCallback channel_a_callback_{nullptr}; +static andino::InterruptIn::InterruptCallback channel_b_callback_{nullptr}; + +TEST_F(EncoderTest, Initialize) { + EXPECT_CALL(channel_a_interrupt_in_, begin()).Times(1); + EXPECT_CALL(channel_b_interrupt_in_, begin()).Times(1); + EXPECT_CALL(channel_a_interrupt_in_, attach(_)) + .Times(1) + .WillOnce(::testing::SaveArg<0>(&channel_a_callback_)); + EXPECT_CALL(channel_b_interrupt_in_, attach(_)) + .Times(1) + .WillOnce(::testing::SaveArg<0>(&channel_b_callback_)); + + encoder_.begin(); + + // Current implementation requires both channels to call the same callback function. + EXPECT_NE(channel_a_callback_, nullptr); + EXPECT_NE(channel_b_callback_, nullptr); + EXPECT_EQ(channel_a_callback_, channel_b_callback_); +} + +TEST_F(EncoderTest, ReadIncreasingTicksCount) { + EXPECT_CALL(channel_a_interrupt_in_, read()) + .Times(4) + .WillOnce(Return(0)) + .WillOnce(Return(1)) + .WillOnce(Return(1)) + .WillOnce(Return(0)); + EXPECT_CALL(channel_b_interrupt_in_, read()) + .Times(4) + .WillOnce(Return(0)) + .WillOnce(Return(0)) + .WillOnce(Return(1)) + .WillOnce(Return(1)); + + EXPECT_EQ(encoder_.read(), 0); + channel_a_callback_(); + EXPECT_EQ(encoder_.read(), 0); + channel_a_callback_(); + EXPECT_EQ(encoder_.read(), 1); + channel_a_callback_(); + EXPECT_EQ(encoder_.read(), 2); + channel_a_callback_(); + EXPECT_EQ(encoder_.read(), 3); +} + +TEST_F(EncoderTest, ReadDecreasingTicksCount) { + EXPECT_CALL(channel_a_interrupt_in_, read()) + .Times(4) + .WillOnce(Return(0)) + .WillOnce(Return(0)) + .WillOnce(Return(1)) + .WillOnce(Return(1)); + EXPECT_CALL(channel_b_interrupt_in_, read()) + .Times(4) + .WillOnce(Return(0)) + .WillOnce(Return(1)) + .WillOnce(Return(1)) + .WillOnce(Return(0)); + + EXPECT_EQ(encoder_.read(), 0); + channel_a_callback_(); + EXPECT_EQ(encoder_.read(), 0); + channel_a_callback_(); + EXPECT_EQ(encoder_.read(), -1); + channel_a_callback_(); + EXPECT_EQ(encoder_.read(), -2); + channel_a_callback_(); + EXPECT_EQ(encoder_.read(), -3); +} + +TEST_F(EncoderTest, ResetTicksCount) { + EXPECT_CALL(channel_a_interrupt_in_, read()) + .Times(3) + .WillOnce(Return(0)) + .WillOnce(Return(1)) + .WillOnce(Return(1)); + EXPECT_CALL(channel_b_interrupt_in_, read()) + .Times(3) + .WillOnce(Return(0)) + .WillOnce(Return(0)) + .WillOnce(Return(1)); + + EXPECT_EQ(encoder_.read(), 0); + channel_a_callback_(); + EXPECT_EQ(encoder_.read(), 0); + channel_a_callback_(); + EXPECT_EQ(encoder_.read(), 1); + channel_a_callback_(); + EXPECT_EQ(encoder_.read(), 2); + encoder_.reset(); + EXPECT_EQ(encoder_.read(), 0); +} + +} // namespace +} // namespace test +} // namespace andino + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + if (RUN_ALL_TESTS()) { + } + + // Always return zero-code and allow PlatformIO to parse results. + return 0; +} diff --git a/andino_firmware/test/desktop/test_motor/motor_test.cpp b/andino_firmware/test/desktop/test_motor/motor_test.cpp new file mode 100644 index 00000000..1566ec07 --- /dev/null +++ b/andino_firmware/test/desktop/test_motor/motor_test.cpp @@ -0,0 +1,137 @@ +// BSD 3-Clause License +// +// Copyright (c) 2024, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "motor.h" + +#include +#include + +#include "digital_out.h" +#include "pwm_out.h" + +namespace andino { +namespace test { +namespace { + +class MockDigitalOut : public andino::DigitalOut { + public: + MockDigitalOut(const int gpio_pin) : andino::DigitalOut(gpio_pin) {} + MOCK_METHOD(void, begin, (), (const, override)); + MOCK_METHOD(void, write, (int value), (const, override)); +}; + +class MockPwmOut : public andino::PwmOut { + public: + MockPwmOut(const int gpio_pin) : andino::PwmOut(gpio_pin) {} + MOCK_METHOD(void, begin, (), (const, override)); + MOCK_METHOD(void, write, (int value), (const, override)); +}; + +class MotorTest : public testing::Test { + protected: + MockDigitalOut enable_digital_out_{0}; + MockPwmOut forward_pwm_out_{0}; + MockPwmOut backward_pwm_out_{0}; +}; + +TEST_F(MotorTest, Initialize) { + EXPECT_CALL(enable_digital_out_, begin()).Times(1); + EXPECT_CALL(forward_pwm_out_, begin()).Times(1); + EXPECT_CALL(backward_pwm_out_, begin()).Times(1); + + andino::Motor motor(&enable_digital_out_, &forward_pwm_out_, &backward_pwm_out_); + motor.begin(); +} + +TEST_F(MotorTest, Enable) { + EXPECT_CALL(enable_digital_out_, write(1)).Times(1); + + andino::Motor motor(&enable_digital_out_, &forward_pwm_out_, &backward_pwm_out_); + motor.enable(true); +} + +TEST_F(MotorTest, Disable) { + EXPECT_CALL(enable_digital_out_, write(0)).Times(1); + + andino::Motor motor(&enable_digital_out_, &forward_pwm_out_, &backward_pwm_out_); + motor.enable(false); +} + +TEST_F(MotorTest, SetPositiveSpeed) { + EXPECT_CALL(forward_pwm_out_, write(100)).Times(1); + EXPECT_CALL(backward_pwm_out_, write(0)).Times(1); + + andino::Motor motor(&enable_digital_out_, &forward_pwm_out_, &backward_pwm_out_); + motor.set_speed(100); +} + +TEST_F(MotorTest, SetNegativeSpeed) { + EXPECT_CALL(forward_pwm_out_, write(0)).Times(1); + EXPECT_CALL(backward_pwm_out_, write(100)).Times(1); + + andino::Motor motor(&enable_digital_out_, &forward_pwm_out_, &backward_pwm_out_); + motor.set_speed(-100); +} + +TEST_F(MotorTest, SetZeroSpeed) { + EXPECT_CALL(forward_pwm_out_, write(0)).Times(1); + EXPECT_CALL(backward_pwm_out_, write(0)).Times(1); + + andino::Motor motor(&enable_digital_out_, &forward_pwm_out_, &backward_pwm_out_); + motor.set_speed(0); +} + +TEST_F(MotorTest, SetHigherThanMaximumPositiveSpeed) { + EXPECT_CALL(forward_pwm_out_, write(255)).Times(1); + EXPECT_CALL(backward_pwm_out_, write(0)).Times(1); + + andino::Motor motor(&enable_digital_out_, &forward_pwm_out_, &backward_pwm_out_); + motor.set_speed(500); +} + +TEST_F(MotorTest, SetLowerThanMinimumNegativeSpeed) { + EXPECT_CALL(forward_pwm_out_, write(0)).Times(1); + EXPECT_CALL(backward_pwm_out_, write(255)).Times(1); + + andino::Motor motor(&enable_digital_out_, &forward_pwm_out_, &backward_pwm_out_); + motor.set_speed(-500); +} + +} // namespace +} // namespace test +} // namespace andino + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + if (RUN_ALL_TESTS()) { + } + + // Always return zero-code and allow PlatformIO to parse results. + return 0; +} diff --git a/andino_firmware/test/desktop/test_pid/pid_test.cpp b/andino_firmware/test/desktop/test_pid/pid_test.cpp new file mode 100644 index 00000000..1cc4ca3d --- /dev/null +++ b/andino_firmware/test/desktop/test_pid/pid_test.cpp @@ -0,0 +1,143 @@ +// BSD 3-Clause License +// +// Copyright (c) 2023, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "pid.h" + +#include + +namespace andino { +namespace test { +namespace { + +TEST(PidTest, Initialize) { + andino::Pid pid_controller(1, 0, 0, 1, -100, 100); + int output = 0; + + // Controller is disabled by default, so output variable should remain unchanged. + pid_controller.compute(5, output); + EXPECT_EQ(output, 0); +} + +TEST(PidTest, ComputeOutputProportionalGain) { + andino::Pid pid_controller(3, 0, 0, 1, -100, 100); + int output = 0; + pid_controller.set_setpoint(15); + pid_controller.enable(); + + pid_controller.compute(5, output); + EXPECT_EQ(output, 30); + + pid_controller.compute(10, output); + EXPECT_EQ(output, 60); +} + +TEST(PidTest, ComputeOutputProportionalAndDerivativeGain) { + andino::Pid pid_controller(3, 2, 0, 1, -100, 100); + int output = 0; + pid_controller.set_setpoint(15); + pid_controller.enable(); + + pid_controller.compute(5, output); + EXPECT_EQ(output, 20); + + pid_controller.compute(10, output); + EXPECT_EQ(output, 50); +} + +TEST(PidTest, ComputeOutputProportionalAndIntegralGain) { + andino::Pid pid_controller(3, 0, 1, 1, -100, 100); + int output = 0; + pid_controller.set_setpoint(15); + pid_controller.enable(); + + pid_controller.compute(5, output); + EXPECT_EQ(output, 30); + + pid_controller.compute(10, output); + EXPECT_EQ(output, 70); +} + +TEST(PidTest, ComputeOutputProportionalDerivativeAndIntegralGain) { + andino::Pid pid_controller(3, 2, 1, 1, -100, 100); + int output = 0; + pid_controller.set_setpoint(15); + pid_controller.enable(); + + pid_controller.compute(5, output); + EXPECT_EQ(output, 20); + + pid_controller.compute(10, output); + EXPECT_EQ(output, 60); +} + +TEST(PidTest, Reset) { + andino::Pid pid_controller(3, 0, 0, 1, -100, 100); + int output = 0; + pid_controller.set_setpoint(15); + pid_controller.enable(); + + pid_controller.compute(5, output); + EXPECT_EQ(output, 30); + + // Controller reset, obtained output should be the same as before. + pid_controller.reset(0); + pid_controller.set_setpoint(15); + pid_controller.compute(5, output); + EXPECT_EQ(output, 30); +} + +TEST(PidTest, SetTunings) { + andino::Pid pid_controller(3, 0, 0, 1, -100, 100); + int output = 0; + pid_controller.set_setpoint(15); + pid_controller.enable(); + + pid_controller.compute(5, output); + EXPECT_EQ(output, 30); + + // Proportional gain doubled, obtained output should be doubled. + pid_controller.reset(0); + pid_controller.set_setpoint(15); + pid_controller.set_tunings(6, 0, 0, 1); + pid_controller.compute(5, output); + EXPECT_EQ(output, 60); +} + +} // namespace +} // namespace test +} // namespace andino + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + if (RUN_ALL_TESTS()) { + } + + // Always return zero-code and allow PlatformIO to parse results. + return 0; +} diff --git a/andino_firmware/test/desktop/test_shell/shell_test.cpp b/andino_firmware/test/desktop/test_shell/shell_test.cpp new file mode 100644 index 00000000..1216419d --- /dev/null +++ b/andino_firmware/test/desktop/test_shell/shell_test.cpp @@ -0,0 +1,299 @@ +// BSD 3-Clause License +// +// Copyright (c) 2024, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "shell.h" + +#include +#include + +#include +#include + +#include "serial_stream.h" + +namespace andino { +namespace test { +namespace { + +using ::testing::Return; + +class MockSerialStream : public andino::SerialStream { + public: + MockSerialStream() : andino::SerialStream() {} + MOCK_METHOD(void, begin, (unsigned long baud), (const, override)); + MOCK_METHOD(int, available, (), (const, override)); + MOCK_METHOD(int, read, (), (const, override)); + MOCK_METHOD(size_t, print, (const char* c), (const, override)); + MOCK_METHOD(size_t, print, (char c), (const, override)); + MOCK_METHOD(size_t, print, (unsigned char b, int base), (const, override)); + MOCK_METHOD(size_t, print, (int num, int base), (const, override)); + MOCK_METHOD(size_t, print, (unsigned int num, int base), (const, override)); + MOCK_METHOD(size_t, print, (long num, int base), (const, override)); + MOCK_METHOD(size_t, print, (unsigned long num, int base), (const, override)); + MOCK_METHOD(size_t, print, (double num, int digits), (const, override)); + MOCK_METHOD(size_t, println, (const char* c), (const, override)); + MOCK_METHOD(size_t, println, (char c), (const, override)); + MOCK_METHOD(size_t, println, (unsigned char b, int base), (const, override)); + MOCK_METHOD(size_t, println, (int num, int base), (const, override)); + MOCK_METHOD(size_t, println, (unsigned int num, int base), (const, override)); + MOCK_METHOD(size_t, println, (long num, int base), (const, override)); + MOCK_METHOD(size_t, println, (unsigned long num, int base), (const, override)); + MOCK_METHOD(size_t, println, (double num, int digits), (const, override)); +}; + +class ShellTest : public testing::Test { + protected: + void SetUp() override { + shell.set_serial_stream(&serial_stream_); + shell.set_default_callback(cmd_unknown_cb); + shell.register_command(kCommand1, cmd_1_cb); + shell.register_command(kCommand2, cmd_2_cb); + shell.register_command(kCommand3, cmd_3_cb); + } + + static void cmd_unknown_cb(int argc, char** argv) { + called_callback_ = 0; + save_arguments(argc, argv); + } + + static void cmd_1_cb(int argc, char** argv) { + called_callback_ = 1; + save_arguments(argc, argv); + } + + static void cmd_2_cb(int argc, char** argv) { + called_callback_ = 2; + save_arguments(argc, argv); + } + + static void cmd_3_cb(int argc, char** argv) { + called_callback_ = 3; + save_arguments(argc, argv); + } + + static void save_arguments(int argc, char** argv) { + argc_ = argc; + argv_.assign(argv, argv + argc); + } + + static constexpr const char* kCommand1{"a"}; + static constexpr const char* kCommand2{"ab"}; + static constexpr const char* kCommand3{"cde"}; + + static int called_callback_; + static int argc_; + static std::vector argv_; + + andino::Shell shell; + MockSerialStream serial_stream_; +}; + +int ShellTest::called_callback_{-1}; +int ShellTest::argc_{0}; +std::vector ShellTest::argv_; + +TEST_F(ShellTest, ProcessInputEmpty) { + EXPECT_CALL(serial_stream_, available()).Times(1).WillOnce(Return(0)); + + shell.process_input(); +} + +TEST_F(ShellTest, ProcessInputMessageSingleCharacterCommandSingleArg) { + const std::string input_message{"a\r"}; + const std::vector expected_argv{"a"}; + + int available_call_count = input_message.size() + 1; + EXPECT_CALL(serial_stream_, available()).Times(available_call_count); + ON_CALL(serial_stream_, available()) + .WillByDefault( + testing::Invoke([&available_call_count]() -> int { return --available_call_count; })); + + int input_index = 0; + EXPECT_CALL(serial_stream_, read()).Times(input_message.size()); + ON_CALL(serial_stream_, read()) + .WillByDefault(testing::Invoke( + [input_message, &input_index]() -> int { return input_message.at(input_index++); })); + + shell.process_input(); + + ASSERT_EQ(called_callback_, 1); + ASSERT_EQ(argc_, 1); + EXPECT_THAT(argv_, ::testing::ElementsAreArray(expected_argv)); +} + +TEST_F(ShellTest, ProcessInputMessageUnknownCommand) { + const std::string input_message{"z\r"}; + const std::vector expected_argv{"z"}; + + int available_call_count = input_message.size() + 1; + EXPECT_CALL(serial_stream_, available()).Times(available_call_count); + ON_CALL(serial_stream_, available()) + .WillByDefault( + testing::Invoke([&available_call_count]() -> int { return --available_call_count; })); + + int input_index = 0; + EXPECT_CALL(serial_stream_, read()).Times(input_message.size()); + ON_CALL(serial_stream_, read()) + .WillByDefault(testing::Invoke( + [input_message, &input_index]() -> int { return input_message.at(input_index++); })); + + shell.process_input(); + + ASSERT_EQ(called_callback_, 0); + ASSERT_EQ(argc_, 1); + EXPECT_THAT(argv_, ::testing::ElementsAreArray(expected_argv)); +} + +TEST_F(ShellTest, ProcessInputMessageTwoCharacterCommandSingleArg) { + const std::string input_message{"ab\r"}; + const std::vector expected_argv{"ab"}; + + int available_call_count = input_message.size() + 1; + EXPECT_CALL(serial_stream_, available()).Times(available_call_count); + ON_CALL(serial_stream_, available()) + .WillByDefault( + testing::Invoke([&available_call_count]() -> int { return --available_call_count; })); + + int input_index = 0; + EXPECT_CALL(serial_stream_, read()).Times(input_message.size()); + ON_CALL(serial_stream_, read()) + .WillByDefault(testing::Invoke( + [input_message, &input_index]() -> int { return input_message.at(input_index++); })); + + shell.process_input(); + + ASSERT_EQ(called_callback_, 2); + ASSERT_EQ(argc_, 1); + EXPECT_EQ(argv_.at(0), expected_argv.at(0)); +} + +TEST_F(ShellTest, ProcessInputMessageThreeCharacterCommandSingleArg) { + const std::string input_message{"cde\r"}; + const std::vector expected_argv{"cde"}; + + int available_call_count = input_message.size() + 1; + EXPECT_CALL(serial_stream_, available()).Times(available_call_count); + ON_CALL(serial_stream_, available()) + .WillByDefault( + testing::Invoke([&available_call_count]() -> int { return --available_call_count; })); + + int input_index = 0; + EXPECT_CALL(serial_stream_, read()).Times(input_message.size()); + ON_CALL(serial_stream_, read()) + .WillByDefault(testing::Invoke( + [input_message, &input_index]() -> int { return input_message.at(input_index++); })); + + shell.process_input(); + + ASSERT_EQ(called_callback_, 3); + ASSERT_EQ(argc_, 1); + EXPECT_THAT(argv_, ::testing::ElementsAreArray(expected_argv)); +} + +TEST_F(ShellTest, ProcessInputMessageTwoArgs) { + const std::string input_message{"a 12\r"}; + const std::vector expected_argv{"a", "12"}; + + int available_call_count = input_message.size() + 1; + EXPECT_CALL(serial_stream_, available()).Times(available_call_count); + ON_CALL(serial_stream_, available()) + .WillByDefault( + testing::Invoke([&available_call_count]() -> int { return --available_call_count; })); + + int input_index = 0; + EXPECT_CALL(serial_stream_, read()).Times(input_message.size()); + ON_CALL(serial_stream_, read()) + .WillByDefault(testing::Invoke( + [input_message, &input_index]() -> int { return input_message.at(input_index++); })); + + shell.process_input(); + + ASSERT_EQ(called_callback_, 1); + ASSERT_EQ(argc_, 2); + EXPECT_THAT(argv_, ::testing::ElementsAreArray(expected_argv)); +} + +TEST_F(ShellTest, ProcessInputMessageThreeArgs) { + const std::string input_message{"ab 12 3\r"}; + const std::vector expected_argv{"ab", "12", "3"}; + + int available_call_count = input_message.size() + 1; + EXPECT_CALL(serial_stream_, available()).Times(available_call_count); + ON_CALL(serial_stream_, available()) + .WillByDefault( + testing::Invoke([&available_call_count]() -> int { return --available_call_count; })); + + int input_index = 0; + EXPECT_CALL(serial_stream_, read()).Times(input_message.size()); + ON_CALL(serial_stream_, read()) + .WillByDefault(testing::Invoke( + [input_message, &input_index]() -> int { return input_message.at(input_index++); })); + + shell.process_input(); + + ASSERT_EQ(called_callback_, 2); + ASSERT_EQ(argc_, 3); + EXPECT_THAT(argv_, ::testing::ElementsAreArray(expected_argv)); +} + +TEST_F(ShellTest, ProcessInputMessageFourArgs) { + const std::string input_message{"cde 12 3 456\r"}; + const std::vector expected_argv{"cde", "12", "3", "456"}; + + int available_call_count = input_message.size() + 1; + EXPECT_CALL(serial_stream_, available()).Times(available_call_count); + ON_CALL(serial_stream_, available()) + .WillByDefault( + testing::Invoke([&available_call_count]() -> int { return --available_call_count; })); + + int input_index = 0; + EXPECT_CALL(serial_stream_, read()).Times(input_message.size()); + ON_CALL(serial_stream_, read()) + .WillByDefault(testing::Invoke( + [input_message, &input_index]() -> int { return input_message.at(input_index++); })); + + shell.process_input(); + + ASSERT_EQ(called_callback_, 3); + ASSERT_EQ(argc_, 4); + EXPECT_THAT(argv_, ::testing::ElementsAreArray(expected_argv)); +} + +} // namespace +} // namespace test +} // namespace andino + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + if (RUN_ALL_TESTS()) { + } + + // Always return zero-code and allow PlatformIO to parse results. + return 0; +} diff --git a/andino_gz_classic/urdf/andino_gz_classic.xacro b/andino_gz_classic/urdf/andino_gz_classic.xacro index 032da9c7..ce4fc755 100644 --- a/andino_gz_classic/urdf/andino_gz_classic.xacro +++ b/andino_gz_classic/urdf/andino_gz_classic.xacro @@ -67,7 +67,7 @@ 0.137 - 0.035 + 0.0662 20 diff --git a/andino_hardware/README.md b/andino_hardware/README.md index c1c3dcf2..7551b83c 100644 --- a/andino_hardware/README.md +++ b/andino_hardware/README.md @@ -4,18 +4,19 @@ This package aims to provide the necessary information to the correct assembly o ## Bill of Materials -| Module | Part | Variant | Comments | -|:--|:------------------------|:---------------------|:-------------------------------------------------------:| -| SBC | Raspberry Pi 4 B (4 Gb) | - | - | -| Chassis | 2 x [Print 3d Chassis](./printing_model/chassis/) + [Wheels](https://www.sparkfun.com/products/13259) | [Robot Smart Car Kit](https://www.amazon.com/perseids-Chassis-Encoder-Wheels-Battery/dp/B07DNYQ3PX/ref=sr_1_1?crid=9WUXNUN54JBG&keywords=Smart%2BCar%2BChassis%2BKit&qid=1685739917&sprefix=smart%2Bcar%2Bchassis%2Bkit%2Caps%2C348&sr=8-1&th=1) | - | -| Motors | 2 x [Hobby Motor with Encoder - Metal Gear (DG01D-E)](https://www.sparkfun.com/products/16413) | [Robot Smart Car Kit](https://www.amazon.com/perseids-Chassis-Encoder-Wheels-Battery/dp/B07DNYQ3PX/ref=sr_1_1?crid=9WUXNUN54JBG&keywords=Smart%2BCar%2BChassis%2BKit&qid=1685739917&sprefix=smart%2Bcar%2Bchassis%2Bkit%2Caps%2C348&sr=8-1&th=1)'s motors + sensor hall encoder | Embbebed encoders are recommended for better accuracy | -| Microcontroller | Arduino Nano | Arduino Uno | Nano is easier to mount given its size | -| Motor Driver | [L298N Dual H Bridge](https://www.amazon.com/Bridge-Stepper-Driver-Module-Controller/dp/B09T6K9RFZ/ref=sr_1_4?crid=37YY7JO6C3WVE&keywords=l298&qid=1685740618&sprefix=l29%2Caps%2C277&sr=8-4) | - | - | -| Laser Scanner | [RPLidar A1M8](https://www.robotshop.com/products/rplidar-a1m8-360-degree-laser-scanner-development-kit?_pos=3&_sid=b0aefcea1&_ss=r) | [RPLidar A2M8](https://www.robotshop.com/products/rplidar-a2m8-360-laser-scanner) | - | -| Camera | [Raspi Camera Module V2, 8 MP](https://www.robotshop.com/products/raspberry-pi-camera-module-v2) | - | - | -| Powerbank 5V | - | - | Any powerbank is suitable: Mind size / weight / output current(>=2A) | -| (Optional) Power Step up | [DC - DC boost converter](https://www.amazon.com/0-9-Step-Regulator-DC-Converter/dp/B0C6QTJMFN/ref=sr_1_25?crid=G0FHM4SS5TWX&keywords=dc+step+up+converter&qid=1685741155&sprefix=dc+step+up+conver%2Caps%2C371&sr=8-25) | - | If motors support higher voltage than 5V a step-up(e.g: to 9V) can be added between powerbank(5V) and motor driver | -| Fixing & Mount | M3 bolts/fasteners - M3 Spacers - M2.5/2.0 bolts/fasteners for SBC | - | - | +| Number | Module | Part | Links | Comments | +|:--:|:--:|:-----------------------:|:--------------------:|:-------------------------------------------------------:| +| 1 | SBC | Raspberry Pi 4 B (4 Gb) | [PiShop](https://www.pishop.us/product/raspberry-pi-4-model-b-2gb/), [TiendaTec](https://www.tiendatec.es/raspberry-pi/gama-raspberry-pi/1100-raspberry-pi-4-modelo-b-4gb-5056561800349.html) | If you want better performance you could buy the 8GB model | +| 2 | Chassis | 2 x Print 3d Chassis + Rubber Tyre Wheels | [Chassis](./printing_model/chassis/), [Wheels Sparkfun](https://www.sparkfun.com/products/13259) | - | +| 3 | Motors | 2 x Motor with Encoder | [Sparkfun](https://www.sparkfun.com/products/16413) | - | +| 4 | Microcontroller | Arduino Nano | [Amazon](https://www.amazon.es/RUIZHI-Interfaz-Controlador-Mejorada-Compatible/dp/B0CNGKG4MZ/ref=sr_1_6?dib=eyJ2IjoiMSJ9.gnHfW9VtlEjMns12dAyHXLyFAlaikWpFyoOQJpO0iJBR-zelggQTQ9n001SH_P6NQ9DO3gPetP2krm7GAGvJus6vz4Utqu8Hy1gol0Rq7nmtJITd70ZNi3linf9v1g1iP7MlBx98cBGLVvFy-O2kZnJ63uZDwOZzwz_kExJzUWAxroO3AjufqqGOQHswLfDfjH6jpOJt54xxpCaqurDccId2O0uGKOj6WpPz6iLSubpsPB479SWYPSncxWQzz2kO4VjT6HVzPS2uWi19TS-A9WXVZceLBiz9t25Pf39jiGQ.1sLxrQ94HdIoXBq4VcDFMZhzKoL3wyJoY-U6BmDI6fY&dib_tag=se&keywords=arduino+nano+v3&qid=1714468231&sr=8-6) | You can also use an Arduino Uno, but mind size | +| 5 | Motor Driver | L298N Dual H Bridge | [Amazon](https://www.amazon.com/Bridge-Stepper-Driver-Module-Controller/dp/B09T6K9RFZ/ref=sr_1_4?crid=37YY7JO6C3WVE&keywords=l298&qid=1685740618&sprefix=l29%2Caps%2C277&sr=8-4) | - | +| 6 | Laser Scanner | RPLidar A1M8 | [RobotShop](https://www.robotshop.com/products/rplidar-a1m8-360-degree-laser-scanner-development-kit?_pos=3&_sid=b0aefcea1&_ss=r), [Amazon](https://www.amazon.es/dp/B07VLFGT27?ref_=cm_sw_r_cso_wa_apan_dp_RJ3AZC2XCEVDK0X2DCGA&starsLeft=1&th=1) | - | +| 7 | Camera | Raspi Camera Module V2, 8 MP | [Robotshop](https://www.robotshop.com/products/raspberry-pi-camera-module-v2), [Amazon](https://www.amazon.com/Raspberry-Pi-Camera-Module-Megapixel/dp/B01ER2SKFS?th=1) | - | +| 8 | Electrical Power Supply | Powerbank 5V | [Amazon](https://www.amazon.es/Heganus-Powerbank-10000mAh-port%C3%A1til-pr%C3%A1ctico/dp/B082PPPWXY/ref=asc_df_B082PPPWXY/?tag=googshopes-21&linkCode=df0&hvadid=420334509253&hvpos=&hvnetw=g&hvrand=13392500367381615369&hvpone=&hvptwo=&hvqmt=&hvdev=c&hvdvcmdl=&hvlocint=&hvlocphy=9181150&hvtargid=pla-878722533582&psc=1&mcid=642b7553488f350a8726c7bfb183a667&tag=&ref=&adgrpid=95757266066&hvpone=&hvptwo=&hvadid=420334509253&hvpos=&hvnetw=g&hvrand=13392500367381615369&hvqmt=&hvdev=c&hvdvcmdl=&hvlocint=&hvlocphy=9181150&hvtargid=pla-878722533582) | Any powerbank is suitable: Mind size / weight / output current(>=2A) | +| 9 | (Optional) Power Step up | DC - DC boost converter | [Amazon America](https://www.amazon.com/0-9-Step-Regulator-DC-Converter/dp/B0C6QTJMFN/ref=sr_1_25?crid=G0FHM4SS5TWX&keywords=dc+step+up+converter&qid=1685741155&sprefix=dc+step+up+conver%2Caps%2C371&sr=8-25), [Amazon Europe](https://www.amazon.com/Converter-Adjustable-Voltage-Regulator-Compatible/dp/B089JYBF25/ref=sr_1_3?crid=3EB0RWDAO1UED&dib=eyJ2IjoiMSJ9.OVkOHemqP_yF8PlJmBNcovwOq6TzYQJADN7pCYP7m9hgHNOuzIA3jqIt5kZK9azOh0Nu3D7ucFbFjgBJprKpAQsC1VhKtCS1z6QLs6w0Ht4seE97e8yWkUkP6fPOry_5D1nyfsh0aMc7wLknNr5R9yDWTg6cYralThbLeU8qfIcpq5m66m9luKznRZiv2eUaXvI0rmcQyLKR2Z5NO_xktttAXuvHAnEnBwpk_3LZ1xA.r84ipJcrDH3o24_JEB5q7jNYEzRKyi56VO3e-xi7QXo&dib_tag=se&keywords=dc%2Bstep%2Bup%2Bconverter&qid=1714468875&sprefix=dc%2Bstep%2Bup%2Bconverter%2Caps%2C170&sr=8-3&th=1) | If motors support higher voltage than 5V a step-up(e.g: to 9V) can be added between powerbank(5V) and motor driver | +| 10 | Fixing & Mount | M3 bolts/fasteners - M3 Spacers - M2.5/2.0 bolts/fasteners for SBC | [Mercado Libre](https://articulo.mercadolibre.com.ar/MLA-823234605-kit-tornillos-electronica-500-unid-fresada-philips-m3-oferta-_JM#position=1&search_layout=stack&type=item&tracking_id=2a14497e-a3dc-4a0f-98fb-b3b524117284), [Amazon](https://www.amazon.com/Taiss-620PCS-Metric-Assortment-Washers/dp/B0CWXRG6VL/ref=sr_1_2_sspa?crid=3R3BT7LOQWZ4B&dib=eyJ2IjoiMSJ9.EBY3VtTnCGRri20ECsEwpF2eTrWOhlADXq8Rbv78LP7JVW0giUfPQ5-G3e5cVq7svNoKIPbFGf0jQoImIPuJvU72yWC0XaaXyHE03TjX1zVT-AxcCUr6bvvqnQrrwFNowZjHy2ZibnHX4sDMx3aixEmx5XUGq43KVEID5FIGzTw6xsLQd410DewktxUFWCHLSD8HR8BeAUKcP3mzciuPmc8dcz9TzY5cZ_wYFO-WyEQ.B5-OkrGZbzkIn8cw4Zb_LtQUoxX1qKuiVqI6PTNmpZk&dib_tag=se&keywords=kit+M3+tuercas+y+tornillos&qid=1714469030&sprefix=kit+m3+tuercas+y+tornillos%2Caps%2C149&sr=8-2-spons&sp_csd=d2lkZ2V0TmFtZT1zcF9hdGY&psc=1) | You would also need a set of screwdrivers if you don't have one | +| 11 | Other 3D printed parts | Camera Mount | [3D models](./printing_model/raspi_cam_mount/) | These parts are for fixing the Raspi Cam at the front of the robot | ## Connection Diagram @@ -24,6 +25,9 @@ This package aims to provide the necessary information to the correct assembly o +**Important❗: As of a recent update recent update ([#215](https://github.com/Ekumen-OS/andino/pull/215)), the A4 and A5 connections were moved to A2 and A3. This diagram will be updated soon. Refer to [andino_firmware/src/hw.h](../andino_firmware/src/hw.h) for pinout reference.** + + Some frequent errors: - If one of the motors rotates in the opposite direction (think about the orientation of the motors in the chassis) probably the output(+ and -) of the L298N's output should be toggled. - When moving forward the encoder values should increase while moving backwards they should decrease. If it is happening the other way around probably the A and B encoder signals should be toggled. @@ -295,7 +299,7 @@ Refer to [`usage`](../README.md#usage) section. #### Network Via terminal the wifi connection can be switched by doing: -List available wifi networks: +List available wifi networks: ``` sudo nmcli dev wifi list ``` diff --git a/andino_navigation/README.md b/andino_navigation/README.md index 665fe1ac..12afd129 100644 --- a/andino_navigation/README.md +++ b/andino_navigation/README.md @@ -1,47 +1,32 @@ -# Nav2 with Andino robot -*Note*: Nav2 is only configured to work with the simulation for the moment. - Support to real robot is forthcoming. +# Andino Navigation -## Build +We rely on [Nav2](https://github.com/ros-planning/navigation2) stack in order to navigate Andino. -Install package dependencies: +# Usage -``` -rosdep install --from-paths src -i -y -``` - -Build the package: +## Prerequisites + 1. Run the mobility stack in a real Andino robot or a simulated one: +_Real robot_ ``` -colcon build +ros2 launch andino_bringup andino_robot.launch.py ``` -Note: `--symlink-install` can be added if needed. - -Finally, source the install folder +_Example with Gazebo Classic_ ``` -. install/setup.bash +ros2 launch andino_gz_classic andino_one_robot.launch.py ``` -# Usage - -This package has the next option to be executed. + 1. Provide a recorded map. Refer to [andino_slam](../andino_slam/README.md) to learn how to record a map with Andino. -## Andino with nav2 in Gazebo Simulation +## Run Nav Stack -This launch file use the [turtlebot3_world](https://github.com/ROBOTIS-GIT/turtlebot3_simulations/tree/master) like world example which has an Apache 2 license. - -``` - . /usr/share/gazebo/setup.bash - ros2 launch andino_navigation andino_simulation_navigation.launch.py +```sh +ros2 launch andino_navigation bringup.launch.py map:= ``` -To test the navigation inside rviz: - -- click in 2D pose estimate button and select the initial pose of the robot -- click in nav2 Goal button and select the final point. -- the robot will start to move to the selected goal. +By default, [config file](params/nav2_params.yaml) is used. For using a custom param file use: -You test adding obstacles inside the Gazebo simulation or use the rviz button Waypoint/ nav through Poses mode to select sequential targets. - -This package has been tested with the Andino robot with `diff drive plugin` in gazebo. If you change the world you should change the map but also it is recommended to tune navigation [parameters](params/nav2_params.yaml). +```sh +ros2 launch andino_navigation bringup.launch.py map:= params_file:= +``` diff --git a/andino_navigation/package.xml b/andino_navigation/package.xml index dbe62178..c62d309d 100644 --- a/andino_navigation/package.xml +++ b/andino_navigation/package.xml @@ -14,11 +14,9 @@ launch_ros - andino_gz_classic launch_ros navigation2 nav2_bringup - turtlebot3_gazebo rviz2 ament_lint_auto @@ -28,4 +26,3 @@ ament_cmake - diff --git a/andino_navigation/params/nav2_params.yaml b/andino_navigation/params/nav2_params.yaml index ea5a01bf..86f72734 100644 --- a/andino_navigation/params/nav2_params.yaml +++ b/andino_navigation/params/nav2_params.yaml @@ -37,7 +37,7 @@ amcl: laser_min_range: -1.0 laser_model_type: "likelihood_field" max_beams: 60 - max_particles: 2000 + max_particles: 3000 min_particles: 500 odom_frame_id: "odom" pf_err: 0.05 @@ -130,7 +130,7 @@ bt_navigator_navigate_to_pose_rclcpp_node: controller_server: ros__parameters: use_sim_time: True - controller_frequency: 20.0 + controller_frequency: 10.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -212,8 +212,8 @@ local_costmap: plugins: ["voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 + cost_scaling_factor: 2.0 + inflation_radius: 0.2 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -270,8 +270,8 @@ global_costmap: map_subscribe_transient_local: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 + cost_scaling_factor: 2.0 + inflation_radius: 0.2 always_send_full_costmap: True map_server: @@ -291,13 +291,13 @@ map_saver: planner_server: ros__parameters: - expected_planner_frequency: 20.0 + expected_planner_frequency: 10.0 use_sim_time: True planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" tolerance: 0.5 - use_astar: false + use_astar: true allow_unknown: true smoother_server: @@ -342,7 +342,7 @@ robot_state_publisher: waypoint_follower: ros__parameters: use_sim_time: True - loop_rate: 20 + loop_rate: 10 stop_on_failure: false waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: @@ -353,14 +353,14 @@ waypoint_follower: velocity_smoother: ros__parameters: use_sim_time: True - smoothing_frequency: 20.0 + smoothing_frequency: 10.0 scale_velocities: False feedback: "OPEN_LOOP" - max_velocity: [0.26, 0.0, 1.0] - min_velocity: [-0.26, 0.0, -1.0] + max_velocity: [0.15, 0.0, 1.0] + min_velocity: [-0.15, 0.0, -1.0] max_accel: [2.5, 0.0, 3.2] max_decel: [-2.5, 0.0, -3.2] - odom_topic: "odom" + odom_topic: "/odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 diff --git a/andino_slam/README.md b/andino_slam/README.md index b26f252a..b5058225 100644 --- a/andino_slam/README.md +++ b/andino_slam/README.md @@ -17,3 +17,22 @@ Several configuration can be forwarded to the `slam_toolbox_node`. By default, t ``` ros2 launch andino_slam slam_toolbox_online_async.launch.py slams_param_file:= ``` + +For saving the map you can use `map_saver_cli` node provided by Nav2. +```sh +ros2 run nav2_map_server map_saver_cli -f +``` + +You can modify the threshold for free space (0.25) and occupied space (0.65) by using +`--free` and `--occ` arguments. + +```sh +ros2 run nav2_map_server map_saver_cli --free 0.15 -f +``` +More information at: + - https://navigation.ros.org/configuration/packages/configuring-map-server.html + - https://github.com/ros-planning/navigation2/tree/main/nav2_map_server + + +Once you have the map saved, you can navigate on it! +Go to [`andino_navigation`](../andino_navigation/README.md) to learn how.