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/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 099b5716..1db4ed31 100644 --- a/andino_description/urdf/andino.urdf.xacro +++ b/andino_description/urdf/andino.urdf.xacro @@ -30,6 +30,10 @@ + + + + @@ -89,6 +93,7 @@ + diff --git a/andino_description/urdf/include/andino_control.urdf.xacro b/andino_description/urdf/include/andino_control.urdf.xacro index 8dc8949a..9aee0f29 100644 --- a/andino_description/urdf/include/andino_control.urdf.xacro +++ b/andino_description/urdf/include/andino_control.urdf.xacro @@ -1,34 +1,43 @@ - - - - andino_base/DiffDriveAndino - - left_wheel_joint - right_wheel_joint - /dev/ttyUSB_ARDUINO - 57600 - 1000 - 585 - - - - -5 - 5 - - - - - - - -5 - 5 - - - - - + + + + + + + + andino_base/DiffDriveAndino + ${hardware_props['left_wheel_name']} + ${hardware_props['right_wheel_name']} + ${hardware_props['serial_device']} + ${hardware_props['baud_rate']} + ${hardware_props['timeout']} + ${hardware_props['enc_ticks_per_rev']} + + + + -5 + 5 + + + + + + + -5 + 5 + + + + + + + diff --git a/andino_firmware/README.md b/andino_firmware/README.md index f4337c8b..0cc3b5b5 100644 --- a/andino_firmware/README.md +++ b/andino_firmware/README.md @@ -9,6 +9,9 @@ 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 @@ -34,7 +37,7 @@ Via `serial` connection (57600 baud) it is possible to interact with the microco - Get encoder values: `'e'` - Set open-loop speed for the motors[pwm] `'o '` - - Example to move forward full speed: `'o 255 255'` + - Example to move forward full speed: `'o 255 255'` - Range `[-255 -> 255]` - Set closed-loop speed for the motors[ticks/sec] `'m '` - Important!: See the `Test it!` section. diff --git a/andino_firmware/platformio.ini b/andino_firmware/platformio.ini index f3b40bbe..6283cef5 100644 --- a/andino_firmware/platformio.ini +++ b/andino_firmware/platformio.ini @@ -22,6 +22,12 @@ 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 desktop platforms (for unit testing). [base_desktop] @@ -32,6 +38,7 @@ build_src_filter = + + + + + ; Environment for Arduino Uno. [env:uno] diff --git a/andino_firmware/src/app.cpp b/andino_firmware/src/app.cpp index f9577468..32c45ce7 100644 --- a/andino_firmware/src/app.cpp +++ b/andino_firmware/src/app.cpp @@ -64,7 +64,11 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "app.h" +#include +#include #include +#include +#include #include "commands.h" #include "constants.h" @@ -115,6 +119,10 @@ unsigned long App::last_pid_computation_{0}; unsigned long App::last_set_motors_speed_cmd_{0}; +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(); @@ -142,6 +150,14 @@ void App::setup() { 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() { @@ -171,10 +187,10 @@ void App::adjust_motors_speed() { 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()){ + if (left_pid_controller_.enabled()) { left_motor_.set_speed(left_motor_speed); } - if (right_pid_controller_.enabled()){ + if (right_pid_controller_.enabled()) { right_motor_.set_speed(right_motor_speed); } } @@ -299,4 +315,47 @@ void App::cmd_set_pid_tuning_gains_cb(int argc, char** argv) { 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 937c7827..0bcbef8f 100644 --- a/andino_firmware/src/app.h +++ b/andino_firmware/src/app.h @@ -29,6 +29,8 @@ // 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" @@ -83,6 +85,12 @@ class App { /// Callback method for the `Commands::kSetPidsTuningGains` command. 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); + /// Serial stream. static SerialStreamArduino serial_stream_; @@ -115,11 +123,17 @@ class App { 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 ff3e5ac5..8136f2e3 100644 --- a/andino_firmware/src/commands.h +++ b/andino_firmware/src/commands.h @@ -82,6 +82,10 @@ struct Commands { static constexpr const char* kSetMotorsPwm{"o"}; /// @brief Sets the PIDs tuning gains [format: "kp:kd:ki:ko"]. 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/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/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_hardware/README.md b/andino_hardware/README.md index c1c3dcf2..54dda0e8 100644 --- a/andino_hardware/README.md +++ b/andino_hardware/README.md @@ -4,25 +4,26 @@ 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 ### Motor-Arduino - + 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. @@ -295,7 +296,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_hardware/docs/andino_diagram_arduino.jpg b/andino_hardware/docs/andino_diagram_arduino.jpg new file mode 100644 index 00000000..0159cd89 Binary files /dev/null and b/andino_hardware/docs/andino_diagram_arduino.jpg differ diff --git a/andino_hardware/docs/arduino_motor_diagram.png b/andino_hardware/docs/arduino_motor_diagram.png deleted file mode 100644 index 5897a801..00000000 Binary files a/andino_hardware/docs/arduino_motor_diagram.png and /dev/null differ