diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 61fac75..58e1d63 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -2,6 +2,7 @@ FROM python:3.9-slim AS stm32flash_builder ARG USERNAME=runner +ENV USER=runner ARG USER_UID=1001 ARG USER_GID=$USER_UID @@ -30,4 +31,6 @@ RUN git clone https://github.com/husarion/flasher && \ # Build stm32 flasher RUN git clone https://github.com/husarion/stm32loader && \ - cd stm32loader && sudo python setup.py install \ No newline at end of file + cd stm32loader && sudo python setup.py install + +SHELL ["/bin/bash", "-c"] diff --git a/.gitmodules b/.gitmodules index 42d11d2..ff91678 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,7 +1,3 @@ -[submodule "ros_lib"] - path = lib/ros_lib - url = https://github.com/byq77/rosserial-mbed.git - branch = master [submodule "encoder-mbed"] path = lib/encoder-mbed url = https://github.com/byq77/encoder-mbed.git @@ -10,11 +6,10 @@ path = lib/motor-driver-mbed url = https://github.com/byq77/drv88xx-driver-mbed.git branch = master -[submodule "imu-driver"] - path = lib/imu-driver - url = https://github.com/byq77/core2-imu-driver.git - branch = master [submodule "vl53l0x-mbed"] path = lib/vl53l0x-mbed url = https://github.com/byq77/vl53l0x-mbed.git branch = master +[submodule "lib/imu-driver"] + path = lib/imu-driver + url = https://github.com/delihus/core2-imu-driver-mbed diff --git a/ignore_packages.py b/ignore_packages.py index e1f0f55..7f27fd3 100644 --- a/ignore_packages.py +++ b/ignore_packages.py @@ -1,5 +1,8 @@ import os user_name = os.getenv('USER') +if user_name == None: + print("There is no global variable 'USER'!!!") + exit(-1) try: os.makedirs(f'/home/{user_name}/.platformio/packages/framework-mbed/features/') except FileExistsError: @@ -8,4 +11,4 @@ with open(f'/home/{user_name}/.platformio/packages/framework-mbed/features/.mbedignore', 'w') as f: f.write('cellular/*\n') f.write('netsocket/cellular/*\n') - f.write('nanostack/*\n') \ No newline at end of file + f.write('nanostack/*\n') diff --git a/include/battery.hpp b/include/battery.hpp new file mode 100644 index 0000000..d6e814e --- /dev/null +++ b/include/battery.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include + +#include +#include +#include + +constexpr int MEASUREMENT_SERIES = 10; +constexpr float BATTERY_VOLTAGE_LOW = 10.8; + +enum +{ + BATTERY_LOW = 1, + BATTERY_OK = 0 +}; + +struct BatteryData +{ + float voltage; + float threshold; + uint8_t status; +}; + +static BatteryData battery_data = { 0.0, BATTERY_VOLTAGE_LOW, BATTERY_OK }; +static DigitalOut battery_led(LED1, 1); +static Ticker battery_led_flipper; +static AnalogIn battery_adc(BAT_MEAS); + +extern sensor_msgs__msg__BatteryState battery_msg; +static rtos::Thread read_battery_voltage_and_show_status_thead; + +void init_battery(); +float read_battery_voltage_and_show_status(); +void read_battery_voltage_and_show_status_task(); +void fill_battery_msg(sensor_msgs__msg__BatteryState *msg); \ No newline at end of file diff --git a/include/buttons.hpp b/include/buttons.hpp new file mode 100644 index 0000000..0b827ea --- /dev/null +++ b/include/buttons.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include + +#include + +enum Buttons +{ + button_left, + button_right, + BUTTONS_COUNT +}; + +static volatile bool buttons_publish_flag[BUTTONS_COUNT] = { false, false }; +static std_msgs__msg__Bool button_msgs[BUTTONS_COUNT]; +void init_button_and_attach_to_callbacks(mbed::InterruptIn *interrupt, void (*rise)(), void (*fall)()); \ No newline at end of file diff --git a/include/imu.hpp b/include/imu.hpp new file mode 100644 index 0000000..ae3cc13 --- /dev/null +++ b/include/imu.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include +#include + +#include +#include +#include + + +static ImuDriver *imu_driver_ptr; +static sensor_msgs__msg__Imu imu_msg; +extern Mail imu_sensor_mail_box; + + +static const char* imu_sensor_type_string[] = { "BNO055_ADDR_A", "BNO055_ADDR_B", "MPU9250", "MPU9255", "BHI260", "UNKNOWN" }; +static char imu_description_string[64] = ""; + +void init_imu(mbed::I2C *i2c_ptr); +void fill_imu_msg(sensor_msgs__msg__Imu *msg); +void fill_imu_msg_with_measurements(sensor_msgs__msg__Imu* msg, ImuDriver::ImuMeasurement* measurements); \ No newline at end of file diff --git a/include/leds.hpp b/include/leds.hpp new file mode 100644 index 0000000..efd5768 --- /dev/null +++ b/include/leds.hpp @@ -0,0 +1,16 @@ +#pragma once +#include +#include + +enum LEDs +{ + led_left, + led_right, + LED_COUNT +}; + +static DigitalOut led2(LED2, 0); +static DigitalOut led3(LED3, 0); + +void led1_callback(const void* message); +void led2_callback(const void* message); diff --git a/include/main.hpp b/include/main.hpp index 313cb8a..9a9f4f9 100644 --- a/include/main.hpp +++ b/include/main.hpp @@ -1,70 +1,7 @@ #pragma once -#include -#include - -#include #include -#include - -#ifndef KINEMATIC_TYPE -#define KINEMATIC_TYPE 0 -#endif - -#ifndef JOINT_STATES_ENABLE -#define JOINT_STATES_ENABLE 0 -#endif -#if JOINT_STATES_ENABLE -static volatile bool joint_states_enabled = true; -#else -static volatile bool joint_states_enabled = false; -#endif static UARTSerial microros_serial(RPI_SERIAL_TX, RPI_SERIAL_RX); static volatile bool distance_sensors_enabled = false; - -static volatile bool buttons_publish_flag[BUTTONS_COUNT] = {false, false}; -static volatile float button_release_time = 0.1; - -static volatile float battery_voltage = 0.0; - -static volatile bool is_speed_watchdog_enabled = true; -static volatile bool is_speed_watchdog_active = false; -static uint64_t speed_watchdog_interval = 1000; // ms - -static Timer odom_watchdog_timer; -static volatile uint32_t last_speed_command_time = 0; - static DigitalOut sens_power(SENS_POWER_ON, 0); - -static InterruptIn button1(BUTTON1); -static InterruptIn button2(BUTTON2); -static ImuDriver *imu_driver_ptr; -static rosbot_sensors::ServoManger servo_manager; - -std::map servo_voltage_configuration{ - {5.0, 0}, - {6.0, 1}, - {7.4, 2}, - {8.6, 3} -}; - -// Motors setup -#define MOTOR_FR MOTOR1 -#define MOTOR_FL MOTOR4 -#define MOTOR_RR MOTOR2 -#define MOTOR_RL MOTOR3 - -constexpr uint8_t POLARITY = 0b00111100; -constexpr float ROBOT_LENGTH = 0.197; -constexpr uint8_t ENCODER_CPR = 48; -constexpr float ROBOT_LENGTH_HALF = ROBOT_LENGTH / 2.0; -constexpr float DISTANCE_FRONT_TO_REAR_WHEEL = 0.11; -constexpr float WHEEL_SEPARATION_LENGTH = DISTANCE_FRONT_TO_REAR_WHEEL / 2; -constexpr float ROBOT_WIDTH = 0.215; // 0.22 0.195 -constexpr float ROBOT_WIDTH_HALF = ROBOT_WIDTH / 2.0; -constexpr float DIAMETER_MODIFICATOR = 1.106; // 1.24, 1.09, 1.164 -constexpr float TYRE_DEFLATION = 1.042; // theoretical distance / real distance -constexpr float GEAR_RATIO = 34.014; -constexpr float WHEEL_DIAMETER = 0.085; -constexpr float WHEEL_RADIUS = WHEEL_DIAMETER / 2.0; \ No newline at end of file diff --git a/include/microros.hpp b/include/microros.hpp index 109097a..2877927 100644 --- a/include/microros.hpp +++ b/include/microros.hpp @@ -7,74 +7,26 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include #include #include +#include +#include +#include +#include +#include +#include +#include + constexpr const uint8_t UXR_CLIENT_DOMAIN_ID_TO_OVERRIDE_WITH_ENV = 255; constexpr const char *NODE_NAME = "rosbot_ros2_firmware"; constexpr const char *IMU_TOPIC_NAME = "_imu/data_raw"; constexpr const char *WHEELS_STATE_TOPIC_NAME = "_motors_response"; -constexpr const char *BATTERY_TOPIC_NAME = "battery"; constexpr const char *WHEELS_COMMAND_TOPIC_NAME = "_motors_cmd"; +constexpr const char *BATTERY_TOPIC_NAME = "battery"; constexpr const char *SERVOS_COMMAND_TOPIC_NAME = "cmd_ser"; -constexpr const char *FRONT_LEFT_MOTOR_NAME = "fl_wheel_joint"; -constexpr const char *FRONT_RIGHT_MOTOR_NAME = "fr_wheel_joint"; -constexpr const char *REAR_LEFT_MOTOR_NAME = "rl_wheel_joint"; -constexpr const char *REAR_RIGHT_MOTOR_NAME = "rr_wheel_joint"; -enum Buttons{ - button_left, - button_right, - BUTTONS_COUNT -}; - -enum Ranges { - range_right_front, - range_left_front, - range_right_rear, - range_left_rear, - RANGES_COUNT -}; - -enum LEDs{ - led_left, - led_right, - LED_COUNT -}; - -enum Motors { - motor_right_rear, - motor_left_rear, - motor_right_front, - motor_left_front, - MOTORS_COUNT -}; - -enum Servos{ - servo0, - servo1, - servo2, - servo3, - servo4, - servo5, - SERVOS_COUNT -}; - -enum MotorsState { - motor_state_position, - motor_state_velocity, - motor_state_effort, - MOTORS_STATE_COUNT -}; - enum AgentStates { WAITING_AGENT, AGENT_AVAILABLE, @@ -82,8 +34,6 @@ enum AgentStates { AGENT_DISCONNECTED }; -static DigitalOut led2(LED2, 0); -static DigitalOut led3(LED3, 0); bool microros_deinit(); #define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){return false;}} @@ -118,13 +68,6 @@ bool init_led_subscribers(); bool init_param_server(); bool init_parameters(); -void fill_wheels_state_msg(sensor_msgs__msg__JointState *msg); -void fill_imu_msg(sensor_msgs__msg__Imu *msg); -void fill_battery_msg(sensor_msgs__msg__BatteryState *msg); -void fill_wheels_command_msg(std_msgs__msg__Float32MultiArray *msg); -void fill_servos_command_msg(std_msgs__msg__UInt32MultiArray *msg); -void fill_range_msg(sensor_msgs__msg__Range *msg, uint8_t id); - bool publish_imu_msg(sensor_msgs__msg__Imu *imu_msg); bool publish_wheels_state_msg(sensor_msgs__msg__JointState *msg); bool publish_battery_msg(sensor_msgs__msg__BatteryState *msg); diff --git a/include/ranges.hpp b/include/ranges.hpp new file mode 100644 index 0000000..7af2410 --- /dev/null +++ b/include/ranges.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include +#include + +#include +#include +#include + +enum Ranges { + range_right_front, + range_left_front, + range_right_rear, + range_left_rear, + RANGES_COUNT +}; + +static const char *range_frame_names[] = {"fr_range", "fl_range", "rr_range", "rl_range"}; + +static sensor_msgs__msg__Range range_msgs[RANGES_COUNT]; + +void init_ranges(); +void fill_range_msg(sensor_msgs__msg__Range *msg, uint8_t id); +void fill_range_msg_with_measurements(sensor_msgs__msg__Range *msg, float range); \ No newline at end of file diff --git a/include/rosbot_sensors.h b/include/rosbot_sensors.h deleted file mode 100644 index 184c906..0000000 --- a/include/rosbot_sensors.h +++ /dev/null @@ -1,135 +0,0 @@ -#ifndef __ROSBOT_SENSORS_H__ -#define __ROSBOT_SENSORS_H__ - -#include -#include - -namespace rosbot_sensors{ - -float updateBatteryWatchdog(); - -class ServoManger : NonCopyable -{ - -public: - enum : int - { - SERVO_OUTPUT_1 = 0, - SERVO_OUTPUT_2 = 1, - SERVO_OUTPUT_3 = 2, - SERVO_OUTPUT_4 = 3, - SERVO_OUTPUT_5 = 4, - SERVO_OUTPUT_6 = 5, - VOLTAGE_5V = 0, - VOLTAGE_6V = 1, - VOLTAGE_7_4V = 2, - VOLTAGE_8_6V = 3, - }; - - ServoManger() - : _servo{nullptr, nullptr, nullptr, nullptr, nullptr, nullptr} - , _voltage_mode(0) - , _enabled_outputs(0) - , _servo_sel1(SERVO_SEL1,0) - , _servo_sel2(SERVO_SEL2,0) - , _servo_power(SERVO_POWER_ON,0) - {} - - void enablePower(bool en = true) - { - _servo_power.write( en ? 1 : 0); - } - - int getEnabledOutputs() - { - return _enabled_outputs; - } - - PwmOut * getOutput(int output) - { - if(output < SERVO_OUTPUT_1 || output > SERVO_OUTPUT_6) - return nullptr; - - return _servo[output]; - } - - bool setWidth(int output, int width_us) - { - if(output < SERVO_OUTPUT_1 || output > SERVO_OUTPUT_6) - return false; - - if(_servo[output] == nullptr) - return false; - - _servo[output]->pulsewidth_us(width_us); - return true; - } - - bool setPeriod(int output, int period_us) - { - if(output < SERVO_OUTPUT_1 || output > SERVO_OUTPUT_6) - return false; - - if(_servo[output] == nullptr) - return false; - - _servo[output]->period_us(period_us); - return true; - } - - void enableOutput(int output, bool en = true) - { - if(output < SERVO_OUTPUT_1 || output > SERVO_OUTPUT_6) - return; - - if(en && _servo[output] == nullptr) - { - switch (output) - { - case SERVO_OUTPUT_1: - _servo[output] = new PwmOut(SERVO1_PWM); - break; - case SERVO_OUTPUT_2: - _servo[output] = new PwmOut(SERVO2_PWM); - break; - case SERVO_OUTPUT_3: - _servo[output] = new PwmOut(SERVO3_PWM); - break; - case SERVO_OUTPUT_4: - _servo[output] = new PwmOut(SERVO4_PWM); - break; - case SERVO_OUTPUT_5: - _servo[output] = new PwmOut(SERVO5_PWM_ALT1); - break; - case SERVO_OUTPUT_6: - _servo[output] = new PwmOut(SERVO6_PWM_ALT1); - break; - } - _enabled_outputs++; - } - else if(_servo[output] != nullptr && !en) - { - delete _servo[output]; - _servo[output] = nullptr; - _enabled_outputs--; - } - } - - void setPowerMode(int mode) - { - _servo_sel1.write(mode & 0x01L); - _servo_sel2.write(mode & 0x02L); - } - -private: - PwmOut *_servo[6]; - int _voltage_mode; - int _enabled_outputs; - DigitalOut _servo_sel1; - DigitalOut _servo_sel2; - DigitalOut _servo_power; -}; - -} - -#endif /* __ROSBOT_SENSORS_H__ */ \ No newline at end of file diff --git a/include/servos.hpp b/include/servos.hpp new file mode 100644 index 0000000..ac3b689 --- /dev/null +++ b/include/servos.hpp @@ -0,0 +1,70 @@ +#pragma once + +#include +#include +#include + +static std::map servo_voltage_configuration{ + {5.0, 0}, + {6.0, 1}, + {7.4, 2}, + {8.6, 3} +}; + +enum Servos{ + servo0, + servo1, + servo2, + servo3, + servo4, + servo5, + SERVOS_COUNT +}; + +extern std_msgs__msg__UInt32MultiArray servos_command_msg; + +void init_servos(); +void fill_servos_command_msg(std_msgs__msg__UInt32MultiArray *msg); +void servos_command_callback(const void* message); + +namespace rosbot_sensors { + +class ServoManger : NonCopyable +{ +public: + enum : int + { + SERVO_OUTPUT_1 = 0, + SERVO_OUTPUT_2 = 1, + SERVO_OUTPUT_3 = 2, + SERVO_OUTPUT_4 = 3, + SERVO_OUTPUT_5 = 4, + SERVO_OUTPUT_6 = 5, + VOLTAGE_5V = 0, + VOLTAGE_6V = 1, + VOLTAGE_7_4V = 2, + VOLTAGE_8_6V = 3, + }; + + ServoManger(); + + void enablePower(bool en = true); + int getEnabledOutputs(); + PwmOut* getOutput(int output); + bool setWidth(int output, int width_us); + bool setPeriod(int output, int period_us); + void enableOutput(int output, bool en = true); + void setPowerMode(int mode); + +private: + PwmOut* _servo[6]; + int _voltage_mode; + int _enabled_outputs; + DigitalOut _servo_sel1; + DigitalOut _servo_sel2; + DigitalOut _servo_power; +}; + +} // namespace rosbot_sensors + +extern rosbot_sensors::ServoManger servo_manager; \ No newline at end of file diff --git a/include/wheels.hpp b/include/wheels.hpp new file mode 100644 index 0000000..f7627ef --- /dev/null +++ b/include/wheels.hpp @@ -0,0 +1,73 @@ +#pragma once + +#include + +#include + +#include +#include +#include +#include + +// Motors setup +#define MOTOR_FR MOTOR1 +#define MOTOR_FL MOTOR4 +#define MOTOR_RR MOTOR2 +#define MOTOR_RL MOTOR3 + +constexpr const char* FRONT_LEFT_MOTOR_NAME = "fl_wheel_joint"; +constexpr const char* FRONT_RIGHT_MOTOR_NAME = "fr_wheel_joint"; +constexpr const char* REAR_LEFT_MOTOR_NAME = "rl_wheel_joint"; +constexpr const char* REAR_RIGHT_MOTOR_NAME = "rr_wheel_joint"; + +static volatile float last_wheels_speed_calc_time = 0.0; +static volatile uint32_t last_speed_command_time = 0; +static volatile bool is_speed_watchdog_active = false; +static volatile uint64_t speed_watchdog_interval = 1000; // ms + +static mbed::Timer odom_watchdog_timer; +static rtos::Thread wheels_state_read_thread; + +constexpr uint8_t POLARITY = 0b00111100; +constexpr float ROBOT_LENGTH = 0.197; +constexpr uint8_t ENCODER_CPR = 48; +constexpr float ROBOT_LENGTH_HALF = ROBOT_LENGTH / 2.0; +constexpr float DISTANCE_FRONT_TO_REAR_WHEEL = 0.11; +constexpr float WHEEL_SEPARATION_LENGTH = DISTANCE_FRONT_TO_REAR_WHEEL / 2; +constexpr float ROBOT_WIDTH = 0.215; // 0.22 0.195 +constexpr float ROBOT_WIDTH_HALF = ROBOT_WIDTH / 2.0; +constexpr float DIAMETER_MODIFICATOR = 1.106; // 1.24, 1.09, 1.164 +constexpr float TYRE_DEFLATION = 1.042; // theoretical distance / real distance +constexpr float GEAR_RATIO = 34.014; +constexpr float WHEEL_DIAMETER = 0.085; +constexpr float WHEEL_RADIUS = WHEEL_DIAMETER / 2.0; + +static sensor_msgs__msg__JointState wheels_state_msg; +extern std_msgs__msg__Float32MultiArray wheels_command_msg; + +enum Motors +{ + motor_right_rear, + motor_left_rear, + motor_right_front, + motor_left_front, + MOTORS_COUNT +}; + +enum MotorsState +{ + motor_state_position, + motor_state_velocity, + motor_state_effort, + MOTORS_STATE_COUNT +}; + +void init_wheels(); +void update_wheels_states(); +void check_speed_watchdog(); +void wheels_state_read_and_watchdog_task(); + +void wheels_command_callback(const void* message); + +void fill_wheels_command_msg(std_msgs__msg__Float32MultiArray *msg); +void fill_wheels_state_msg(sensor_msgs__msg__JointState* msg); \ No newline at end of file diff --git a/lib/imu-driver b/lib/imu-driver index 0007942..a3a988a 160000 --- a/lib/imu-driver +++ b/lib/imu-driver @@ -1 +1 @@ -Subproject commit 0007942dea070528956629c900175d2679ce9017 +Subproject commit a3a988a65036a54ed7fa96d5de6d65854ec8303c diff --git a/src/battery.cpp b/src/battery.cpp new file mode 100644 index 0000000..ed40865 --- /dev/null +++ b/src/battery.cpp @@ -0,0 +1,59 @@ +#include + +sensor_msgs__msg__BatteryState battery_msg; + +void init_battery() +{ + read_battery_voltage_and_show_status_thead.start(read_battery_voltage_and_show_status_task); + fill_battery_msg(&battery_msg); +} + +void read_battery_voltage_and_show_status_task() +{ + while (true) + { + battery_msg.voltage = read_battery_voltage_and_show_status(); + ThisThread::sleep_for(100); + } +} + +static void toggle_battery_led() +{ + battery_led = !battery_led; +} + +float read_battery_voltage_and_show_status() +{ + static int index = 0; + battery_data.voltage = + 3.3f * VIN_MEAS_CORRECTION * (UPPER_RESISTOR + LOWER_RESISTOR) / LOWER_RESISTOR * battery_adc.read(); + if (battery_data.threshold > battery_data.voltage && index < MEASUREMENT_SERIES) // low level + index++; + else if (battery_data.threshold < battery_data.voltage && index > 0) + index--; + + if (battery_data.status == BATTERY_OK && index == MEASUREMENT_SERIES) + { + battery_data.status = BATTERY_LOW; + battery_led = 0; + battery_led_flipper.attach(callback(toggle_battery_led), 0.4); + } + else if (battery_data.status == BATTERY_LOW && index == 0) + { + battery_data.status = BATTERY_OK; + battery_led_flipper.detach(); + battery_led = 1; + } + return battery_data.voltage; +} + +void fill_battery_msg(sensor_msgs__msg__BatteryState *msg) { + msg->header.frame_id = micro_ros_string_utilities_set(msg->header.frame_id, "battery"); + + if (rmw_uros_epoch_synchronized()) { + msg->header.stamp.sec = (int32_t)(rmw_uros_epoch_nanos() / 1000000000); + msg->header.stamp.nanosec = (uint32_t)(rmw_uros_epoch_nanos() % 1000000000); + } + + msg->power_supply_technology = sensor_msgs__msg__BatteryState__POWER_SUPPLY_TECHNOLOGY_LION; +} \ No newline at end of file diff --git a/src/buttons.cpp b/src/buttons.cpp new file mode 100644 index 0000000..cff9921 --- /dev/null +++ b/src/buttons.cpp @@ -0,0 +1,7 @@ +#include "buttons.hpp" + +void init_button_and_attach_to_callbacks(mbed::InterruptIn *interrupt, void (*rise)(), void (*fall)()){ + interrupt->mode(PullUp); + interrupt->fall(fall); + interrupt->rise(rise); +} \ No newline at end of file diff --git a/src/imu.cpp b/src/imu.cpp new file mode 100644 index 0000000..27d1f8f --- /dev/null +++ b/src/imu.cpp @@ -0,0 +1,60 @@ +#include + +void init_imu(mbed::I2C* i2c_ptr) +{ + ImuDriver::Type type = ImuDriver::getType(i2c_ptr, 2); + sprintf(imu_description_string, "Detected sensor: %s\r\n", imu_sensor_type_string[type]); + + if (type != ImuDriver::UNKNOWN) + { + imu_driver_ptr = new ImuDriver(i2c_ptr, type); + imu_driver_ptr->init(); + imu_driver_ptr->start(); + } + + fill_imu_msg(&imu_msg); +} + +void fill_imu_msg(sensor_msgs__msg__Imu* msg) +{ + msg->header.frame_id = micro_ros_string_utilities_set(msg->header.frame_id, "imu"); + + if (rmw_uros_epoch_synchronized()) + { + msg->header.stamp.sec = (int32_t)(rmw_uros_epoch_nanos() / 1000000000); + msg->header.stamp.nanosec = (uint32_t)(rmw_uros_epoch_nanos() % 1000000000); + } + msg->orientation.x = 0; + msg->orientation.y = 0; + msg->orientation.z = 0; + msg->orientation.w = 1; + msg->angular_velocity.x = 0; + msg->angular_velocity.y = 0; + msg->angular_velocity.z = 0; + msg->linear_acceleration.x = 0; + msg->linear_acceleration.y = 0; + msg->linear_acceleration.z = 0; + for (auto i = 0u; i < 9u; ++i) + { + msg->angular_velocity_covariance[i] = msg->linear_acceleration_covariance[i] = msg->orientation_covariance[i] = 0.0; + } + msg->orientation_covariance[9] = 0.0; + msg->orientation_covariance[10] = 0.0; + msg->orientation_covariance[11] = 0.0; +} + +void fill_imu_msg_with_measurements(sensor_msgs__msg__Imu* msg, ImuDriver::ImuMeasurement* measurements) +{ + msg->orientation.y = measurements->orientation[1]; + msg->orientation.z = measurements->orientation[2]; + msg->orientation.x = measurements->orientation[0]; + msg->orientation.w = measurements->orientation[3]; + + msg->angular_velocity.x = measurements->angular_velocity[0]; + msg->angular_velocity.y = measurements->angular_velocity[1]; + msg->angular_velocity.z = measurements->angular_velocity[2]; + + msg->linear_acceleration.x = measurements->linear_acceleration[0]; + msg->linear_acceleration.y = measurements->linear_acceleration[1]; + msg->linear_acceleration.z = measurements->linear_acceleration[2]; +} \ No newline at end of file diff --git a/src/leds.cpp b/src/leds.cpp new file mode 100644 index 0000000..ae62299 --- /dev/null +++ b/src/leds.cpp @@ -0,0 +1,19 @@ +#include "leds.hpp" + +void led1_callback(const void* msgin) +{ + const std_msgs__msg__Bool* msg = (const std_msgs__msg__Bool*)msgin; + if (msg != NULL) + { + led2 = msg->data; + } +} + +void led2_callback(const void* msgin) +{ + const std_msgs__msg__Bool* msg = (const std_msgs__msg__Bool*)msgin; + if (msg != NULL) + { + led3 = msg->data; + } +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 1236f53..dde9ce6 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -4,7 +4,6 @@ * @author Husarion * @copyright MIT */ -#include #include @@ -13,376 +12,228 @@ #define IMU_I2C_SCL SENS2_PIN3 #define IMU_I2C_SDA SENS2_PIN4 -extern Mail imu_sensor_mail_box; - -const char *imu_sensor_type_string[] = { - "BNO055_ADDR_A", - "BNO055_ADDR_B", - "MPU9250", - "MPU9255", - "UNKNOWN"}; -char imu_description_string[64] = ""; - -static float curr_odom_calc_time = 0.0; -static float last_odom_calc_time = 0.0; volatile uint8_t err_msg; +static volatile uint32_t spin_count; -sensor_msgs__msg__Imu imu_msg; -sensor_msgs__msg__BatteryState battery_msg; -sensor_msgs__msg__JointState wheels_state_msg; -sensor_msgs__msg__Range range_msgs[RANGES_COUNT]; -std_msgs__msg__Bool button_msgs[BUTTONS_COUNT]; +static mbed::InterruptIn button1(BUTTON1); +static mbed::InterruptIn button2(BUTTON2); -static uint32_t spin_count = 1; - -static void button1FallCallback() { - buttons_publish_flag[0] = true; +void button1_fall_callback() +{ + buttons_publish_flag[0] = true; } -static void button2FallCallback() { - buttons_publish_flag[1] = true; +void button2_fall_callback() +{ + buttons_publish_flag[1] = true; } -static void button1RiseCallback() { - buttons_publish_flag[0] = false; +void button1_rise_callback() +{ + buttons_publish_flag[0] = false; } -static void button2RiseCallback() { - buttons_publish_flag[1] = false; +void button2_rise_callback() +{ + buttons_publish_flag[1] = false; } -void range_sensors_msg_handler() { - osEvent evt1 = distance_sensor_mail_box.get(0); - if (evt1.status == osEventMail) { - SensorsMeasurement *message = (SensorsMeasurement *)evt1.value.p; - if (message->status == MultiDistanceSensor::ERR_I2C_FAILURE) { - err_msg++; - if (distance_sensor_commands.empty() && err_msg == 3) { - uint8_t *data = distance_sensor_commands.alloc(); - *data = 2; - distance_sensor_commands.put(data); - data = distance_sensor_commands.alloc(); - *data = 1; - distance_sensor_commands.put(data); - err_msg = 0; - } - - } else { - err_msg = 0; - for (auto i = 0u; i < RANGES_COUNT; ++i) { - range_msgs[i].range = message->range[i]; - } - } - distance_sensor_mail_box.free(message); +void range_sensors_msg_handler() +{ + osEvent evt1 = distance_sensor_mail_box.get(0); + if (evt1.status == osEventMail) + { + SensorsMeasurement* message = (SensorsMeasurement*)evt1.value.p; + if (message->status == MultiDistanceSensor::ERR_I2C_FAILURE) + { + err_msg++; + if (distance_sensor_commands.empty() && err_msg == 3) + { + uint8_t* data = distance_sensor_commands.alloc(); + *data = 2; + distance_sensor_commands.put(data); + data = distance_sensor_commands.alloc(); + *data = 1; + distance_sensor_commands.put(data); + err_msg = 0; + } } -} - -void publish_range_sensors(rcl_timer_t *timer, int64_t last_call_time) { - RCLC_UNUSED(last_call_time); - if (timer != NULL) { - for (auto i = 0u; i < RANGES_COUNT; ++i) { - fill_range_msg(&range_msgs[i], i); - publish_range_msg(&range_msgs[i], i); - } + else + { + err_msg = 0; + for (auto i = 0u; i < RANGES_COUNT; ++i) + { + fill_range_msg_with_measurements(&range_msgs[i], message->range[i]); + } } + distance_sensor_mail_box.free(message); + } } -void imu_msg_handler() { - osEvent evt2 = imu_sensor_mail_box.get(0); - - if (evt2.status == osEventMail) { - ImuDriver::ImuMesurement *message = (ImuDriver::ImuMesurement *)evt2.value.p; - fill_imu_msg(&imu_msg); - imu_msg.orientation.y = message->orientation[1]; - imu_msg.orientation.z = message->orientation[2]; - imu_msg.orientation.x = message->orientation[0]; - imu_msg.orientation.w = message->orientation[3]; - - imu_msg.angular_velocity.x = message->angular_velocity[0]; - imu_msg.angular_velocity.y = message->angular_velocity[1]; - imu_msg.angular_velocity.z = message->angular_velocity[2]; - - imu_msg.linear_acceleration.x = message->linear_acceleration[0]; - imu_msg.linear_acceleration.y = message->linear_acceleration[1]; - imu_msg.linear_acceleration.z = message->linear_acceleration[2]; - publish_imu_msg(&imu_msg); - imu_sensor_mail_box.free(message); +void publish_range_sensors(rcl_timer_t* timer, int64_t last_call_time) +{ + RCLC_UNUSED(last_call_time); + if (timer != NULL) + { + for (auto i = 0u; i < RANGES_COUNT; ++i) + { + fill_range_msg(&range_msgs[i], i); + publish_range_msg(&range_msgs[i], i); } + } } -void battery_msg_handler() { - if (spin_count % 40 == 0) { - fill_battery_msg(&battery_msg); - battery_msg.voltage = battery_voltage; - publish_battery_msg(&battery_msg); - } +void imu_msg_handler() +{ + osEvent evt2 = imu_sensor_mail_box.get(0); + + if (evt2.status == osEventMail) + { + ImuDriver::ImuMeasurement* message = (ImuDriver::ImuMeasurement*)evt2.value.p; + fill_imu_msg(&imu_msg); + fill_imu_msg_with_measurements(&imu_msg, message); + publish_imu_msg(&imu_msg); + imu_sensor_mail_box.free(message); + } } -void button_msgs_handler() { - for(auto i = 0u; i < BUTTONS_COUNT; ++i){ - if (buttons_publish_flag[i] != button_msgs[i].data) { - button_msgs[i].data = buttons_publish_flag[i]; - publish_button_msg(&button_msgs[i], i); - } - } +void battery_msg_handler() +{ + if (spin_count % 40 == 0) + { + fill_battery_msg(&battery_msg); + publish_battery_msg(&battery_msg); + } } -void wheels_state_msg_handler() { - if (spin_count % 5 == 0) { - publish_wheels_state_msg(&wheels_state_msg); +void button_msgs_handler() +{ + for (auto i = 0u; i < BUTTONS_COUNT; ++i) + { + if (buttons_publish_flag[i] != button_msgs[i].data) + { + button_msgs[i].data = buttons_publish_flag[i]; + publish_button_msg(&button_msgs[i], i); } + } } -void read_and_show_battery_state() { - battery_voltage = rosbot_sensors::updateBatteryWatchdog(); +void wheels_state_msg_handler() +{ + if (spin_count % 5 == 0) + { + fill_wheels_state_msg(&wheels_state_msg); + publish_wheels_state_msg(&wheels_state_msg); + } } -void check_speed_watchdog() { - if (is_speed_watchdog_enabled) { - if (!is_speed_watchdog_active && (odom_watchdog_timer.read_ms() - last_speed_command_time) > speed_watchdog_interval) { - RosbotDrive &drive = RosbotDrive::getInstance(); - NewTargetSpeed new_speed = { - .speed = {0.0, 0.0, 0.0, 0.0}, - .mode = MPS - }; - drive.updateTargetSpeed(new_speed); - is_speed_watchdog_active = true; +void timer_callback(rcl_timer_t* timer, int64_t last_call_time) +{ + RCLC_UNUSED(last_call_time); + if (timer != NULL) + { + imu_msg_handler(); + wheels_state_msg_handler(); + button_msgs_handler(); + range_sensors_msg_handler(); + battery_msg_handler(); + spin_count++; + } +} + +bool on_parameter_changed(const Parameter* old_param, const Parameter* new_param, void* context) +{ + (void)context; + if (old_param == NULL && new_param == NULL) + { + return false; + } + if (old_param != NULL and new_param != NULL) + { + std::map::iterator it; + switch (old_param->value.type) + { + case RCLC_PARAMETER_DOUBLE: + it = servo_voltage_configuration.find(new_param->value.double_value); + if (it == servo_voltage_configuration.end()) + { + return false; } - } -} - -void update_odometry() { - if (spin_count % 2 == 0) { - RosbotDrive &drive = RosbotDrive::getInstance(); - - float current_position[MOTORS_COUNT]; - current_position[motor_left_front] = drive.getAngularPos(MOTOR_FL); - current_position[motor_right_front] = drive.getAngularPos(MOTOR_FR); - current_position[motor_left_rear] = drive.getAngularPos(MOTOR_RL); - current_position[motor_right_rear] = drive.getAngularPos(MOTOR_RR); - - curr_odom_calc_time = odom_watchdog_timer.read(); - double dt = curr_odom_calc_time - last_odom_calc_time; - last_odom_calc_time = curr_odom_calc_time; - - for (auto i = 0u; i < MOTORS_COUNT; ++i) { - wheels_state_msg.velocity.data[i] = (current_position[i] - wheels_state_msg.position.data[i]) / dt; - wheels_state_msg.position.data[i] = current_position[i]; + servo_manager.setPowerMode(it->second); + break; + case RCLC_PARAMETER_BOOL: + if (not strcmp(new_param->name.data, "servo_enable_power")) + { + servo_manager.enablePower(new_param->value.bool_value); } - } -} - -void wheels_command_callback(const void *msgin) { - const std_msgs__msg__Float32MultiArray *msg = (const std_msgs__msg__Float32MultiArray *)msgin; - if (msg != NULL and msg->data.size == MOTORS_COUNT) { - RosbotDrive &drive = RosbotDrive::getInstance(); - NewTargetSpeed new_speed = { - .speed = { - msg->data.data[motor_right_front] * WHEEL_RADIUS, - msg->data.data[motor_right_rear] * WHEEL_RADIUS, - msg->data.data[motor_left_rear] * WHEEL_RADIUS, - msg->data.data[motor_left_front] * WHEEL_RADIUS, - }, - .mode = MPS - }; - - for(auto i = 0u; i < MOTORS_COUNT; ++i){ - if(isnan(new_speed.speed[i])){ - float zero_speeds[MOTORS_COUNT] = {0, 0, 0, 0}; - memcpy(new_speed.speed, zero_speeds, sizeof(new_speed.speed)); - break; - } + else if (isdigit(new_param->name.data[5])) + { + servo_manager.enableOutput(new_param->name.data[5] - '0', new_param->value.bool_value); } - - drive.updateTargetSpeed(new_speed); - last_speed_command_time = odom_watchdog_timer.read_ms(); - is_speed_watchdog_active = false; - } -} - -void servos_command_callback(const void *msgin) { - const std_msgs__msg__UInt32MultiArray *msg = (const std_msgs__msg__UInt32MultiArray *)msgin; - if (msg != NULL and msg->data.size == SERVOS_COUNT) { - for(auto i = 0u; i < SERVOS_COUNT; ++i){ - servo_manager.setWidth(i, msg->data.data[i]); + else + { + return false; } - } -} - -void led1_callback(const void *msgin) { - const std_msgs__msg__Bool *msg = (const std_msgs__msg__Bool *)msgin; - if (msg != NULL) { - led2 = msg->data; - } -} - -void led2_callback(const void *msgin) { - const std_msgs__msg__Bool *msg = (const std_msgs__msg__Bool *)msgin; - if (msg != NULL) { - led3 = msg->data; - } -} - -void odometry_callback() { - while (true) { - check_speed_watchdog(); - read_and_show_battery_state(); - update_odometry(); - ThisThread::sleep_for(10); - } -} - -void timer_callback(rcl_timer_t *timer, int64_t last_call_time) { - RCLC_UNUSED(last_call_time); - if (timer != NULL) { - imu_msg_handler(); - wheels_state_msg_handler(); - button_msgs_handler(); - range_sensors_msg_handler(); - battery_msg_handler(); - button_msgs_handler(); - spin_count++; - } -} - -bool on_parameter_changed(const Parameter * old_param, const Parameter * new_param, void * context) -{ - (void) context; - if (old_param == NULL && new_param == NULL) { - return false; - } - if(old_param != NULL and new_param != NULL){ - std::map::iterator it; - switch (old_param->value.type) { - case RCLC_PARAMETER_DOUBLE: - it = servo_voltage_configuration.find(new_param->value.double_value); - if (it == servo_voltage_configuration.end()){ - return false; - } - servo_manager.setPowerMode(it->second); - break; - case RCLC_PARAMETER_BOOL: - if(not strcmp(new_param->name.data, "servo_enable_power")){ - servo_manager.enablePower(new_param->value.bool_value); - } - else if (isdigit(new_param->name.data[5])){ - servo_manager.enableOutput(new_param->name.data[5] - '0', new_param->value.bool_value); - } - else{ - return false; - } - break; - case RCLC_PARAMETER_INT: - if (isdigit(new_param->name.data[5])){ - servo_manager.setPeriod(new_param->name.data[5] - '0', new_param->value.integer_value); - } - else{ - return false; - } - break; - default: - break; + break; + case RCLC_PARAMETER_INT: + if (isdigit(new_param->name.data[5])) + { + servo_manager.setPeriod(new_param->name.data[5] - '0', new_param->value.integer_value); } + else + { + return false; + } + break; + default: + break; } + } return true; } -int main() { - ThisThread::sleep_for(100); - sens_power = 1; // sensors power on +int main() +{ + ThisThread::sleep_for(100); + sens_power = 1; // sensors power on + ThisThread::sleep_for(100); + init_battery(); + init_wheels(); + init_button_and_attach_to_callbacks(&button1, button1_rise_callback, button1_fall_callback); + init_button_and_attach_to_callbacks(&button2, button2_rise_callback, button2_fall_callback); + + I2C* i2c_ptr = new I2C(IMU_I2C_SDA, IMU_I2C_SCL); + i2c_ptr->frequency(IMU_I2C_FREQUENCY); + init_imu(i2c_ptr); + init_servos(); + init_ranges(); + + set_microros_serial_transports(µros_serial); + while (not rmw_uros_ping_agent(100, 1) == RMW_RET_OK) + { ThisThread::sleep_for(100); - odom_watchdog_timer.start(); - - RosbotDrive &drive = RosbotDrive::getInstance(); - MultiDistanceSensor &distance_sensors = MultiDistanceSensor::getInstance(); - - RosbotWheel custom_wheel_params = { - .radius = WHEEL_RADIUS, - .diameter_modificator = DIAMETER_MODIFICATOR, - .tyre_deflation = TYRE_DEFLATION, - .gear_ratio = GEAR_RATIO, - .encoder_cpr = ENCODER_CPR, - .polarity = POLARITY}; - - drive.setupMotorSequence(MOTOR_FR, MOTOR_FL, MOTOR_RR, MOTOR_RL); - drive.init(custom_wheel_params, RosbotDrive::DEFAULT_REGULATOR_PARAMS); - drive.enable(true); - drive.enablePidReg(true); - - button1.mode(PullUp); - button2.mode(PullUp); - button1.fall(button1FallCallback); - button2.fall(button2FallCallback); - button1.rise(button1RiseCallback); - button2.rise(button2RiseCallback); - - - bool distance_sensors_init_flag = false; - bool imu_init_flag = false; - - int num_sens_init; - if ((num_sens_init = distance_sensors.init()) > 0) { - distance_sensors_init_flag = true; - } - - I2C *i2c_ptr = new I2C(IMU_I2C_SDA, IMU_I2C_SCL); - i2c_ptr->frequency(IMU_I2C_FREQUENCY); + } - ImuDriver::Type type = ImuDriver::getType(i2c_ptr, 2); - sprintf(imu_description_string, "Detected sensor: %s\r\n", imu_sensor_type_string[type]); - - if (type != ImuDriver::UNKNOWN) { - imu_driver_ptr = new ImuDriver(i2c_ptr, type); - imu_driver_ptr->init(); - imu_driver_ptr->start(); - imu_init_flag = true; - } - - Thread odometry_thread; - odometry_thread.start(odometry_callback); - - if (imu_init_flag) { - imu_driver_ptr->start(); - } - - if (distance_sensors_init_flag) { - uint8_t *data = distance_sensor_commands.alloc(); - *data = 1; - distance_sensor_commands.put(data); - distance_sensors_enabled = true; - } - - read_and_show_battery_state(); - - set_microros_serial_transports(µros_serial); - while (not rmw_uros_ping_agent(100, 1) == RMW_RET_OK) { - read_and_show_battery_state(); - ThisThread::sleep_for(100); - } - - if (not microros_init()) { - microros_deinit(); - ThisThread::sleep_for(2000); - - NVIC_SystemReset(); - } - - fill_imu_msg(&imu_msg); - fill_battery_msg(&battery_msg); - fill_wheels_state_msg(&wheels_state_msg); - - for (auto i = 0u; i < RANGES_COUNT; ++i) { - fill_range_msg(&range_msgs[i], i); - } - - AgentStates state = AGENT_CONNECTED; - while (state == AGENT_CONNECTED) { - EXECUTE_EVERY_N_MS(2000, state = (RMW_RET_OK == rmw_uros_ping_agent(200, 5)) ? AGENT_CONNECTED : AGENT_DISCONNECTED;); - microros_spin(); - } - for (int i = 0; i < 10; ++i) { - ThisThread::sleep_for(200); - } + if (not microros_init()) + { microros_deinit(); + ThisThread::sleep_for(2000); + NVIC_SystemReset(); + } + + AgentStates state = AGENT_CONNECTED; + while (state == AGENT_CONNECTED) + { + EXECUTE_EVERY_N_MS(2000, + state = (RMW_RET_OK == rmw_uros_ping_agent(200, 5)) ? AGENT_CONNECTED : AGENT_DISCONNECTED;); + microros_spin(); + } + + for (int i = 0; i < 10; ++i) + { + ThisThread::sleep_for(200); + } + microros_deinit(); + NVIC_SystemReset(); } \ No newline at end of file diff --git a/src/microros.cpp b/src/microros.cpp index c4db410..4af7a68 100644 --- a/src/microros.cpp +++ b/src/microros.cpp @@ -20,28 +20,18 @@ rcl_subscription_t led_subs[LED_COUNT]; rclc_parameter_server_t param_server; -std_msgs__msg__Float32MultiArray wheels_command_msg; -std_msgs__msg__UInt32MultiArray servos_command_msg; std_msgs__msg__Bool led_msg; -const char *range_frame_names[] = {"fr_range", "fl_range", "rr_range", "rl_range"}; const char *range_pub_names[] = {"range/fr", "range/fl", "range/rr", "range/rl"}; const char *buttons_pub_names[] = {"button/left", "button/right"}; const char *led_subs_names[] = {"led/left", "led/right"}; extern void timer_callback(rcl_timer_t *timer, int64_t last_call_time); -extern void wheels_command_callback(const void *msgin); -extern void servos_command_callback(const void *msgin); -extern void led1_callback(const void *msgin); -extern void led2_callback(const void *msgin); extern void publish_range_sensors(rcl_timer_t *timer, int64_t last_call_time); extern bool on_parameter_changed(const Parameter * old_param, const Parameter * new_param, void * context); bool microros_init() { - fill_wheels_command_msg(&wheels_command_msg); - fill_servos_command_msg(&servos_command_msg); - rcl_allocator = rcl_get_default_allocator(); rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); @@ -241,100 +231,4 @@ bool publish_range_msg(sensor_msgs__msg__Range *msg, uint8_t id) { bool publish_button_msg(std_msgs__msg__Bool *msg, uint8_t id) { RCCHECK(rcl_publish(&buttons_pubs[id], msg, NULL)); return true; -} - -void fill_wheels_state_msg(sensor_msgs__msg__JointState *msg) { - static double msg_data_tab[MOTORS_STATE_COUNT][MOTORS_COUNT]; - static rosidl_runtime_c__String msg_name_tab[MOTORS_COUNT]; - msg->header.frame_id = micro_ros_string_utilities_set(msg->header.frame_id, "wheels_state"); - - msg->position.data = msg_data_tab[motor_state_position]; - msg->position.capacity = msg->position.size = MOTORS_COUNT; - msg->velocity.data = msg_data_tab[motor_state_velocity]; - msg->velocity.capacity = msg->velocity.size = MOTORS_COUNT; - msg->effort.data = msg_data_tab[motor_state_effort]; - msg->effort.capacity = msg->effort.size = MOTORS_COUNT; - - msg_name_tab->capacity = msg_name_tab->size = MOTORS_COUNT; - msg_name_tab[motor_left_front].data = (char *)FRONT_LEFT_MOTOR_NAME; - msg_name_tab[motor_right_front].data = (char *)FRONT_RIGHT_MOTOR_NAME; - msg_name_tab[motor_left_rear].data = (char *)REAR_LEFT_MOTOR_NAME; - msg_name_tab[motor_right_rear].data = (char *)REAR_RIGHT_MOTOR_NAME; - for (uint8_t i = 0; i < MOTORS_COUNT; i++) { - msg_name_tab[i].capacity = msg_name_tab[i].size = strlen(msg_name_tab[i].data); - } - msg->name.capacity = msg->name.size = MOTORS_COUNT; - msg->name.data = msg_name_tab; - - if (rmw_uros_epoch_synchronized()) { - msg->header.stamp.sec = (int32_t)(rmw_uros_epoch_nanos() / 1000000000); - msg->header.stamp.nanosec = (uint32_t)(rmw_uros_epoch_nanos() % 1000000000); - } -} - -void fill_imu_msg(sensor_msgs__msg__Imu *msg) { - msg->header.frame_id = micro_ros_string_utilities_set(msg->header.frame_id, "imu"); - - if (rmw_uros_epoch_synchronized()) { - msg->header.stamp.sec = (int32_t)(rmw_uros_epoch_nanos() / 1000000000); - msg->header.stamp.nanosec = (uint32_t)(rmw_uros_epoch_nanos() % 1000000000); - } - msg->orientation.x = 0; - msg->orientation.y = 0; - msg->orientation.z = 0; - msg->orientation.w = 1; - msg->angular_velocity.x = 0; - msg->angular_velocity.y = 0; - msg->angular_velocity.z = 0; - msg->linear_acceleration.x = 0; - msg->linear_acceleration.y = 0; - msg->linear_acceleration.z = 0; - for (auto i = 0u; i < 9u; ++i) { - msg->angular_velocity_covariance[i] = msg->linear_acceleration_covariance[i] = msg->orientation_covariance[i] = 0.0; - } - msg->orientation_covariance[9] = 0.0; - msg->orientation_covariance[10] = 0.0; - msg->orientation_covariance[11] = 0.0; -} - -void fill_battery_msg(sensor_msgs__msg__BatteryState *msg) { - msg->header.frame_id = micro_ros_string_utilities_set(msg->header.frame_id, "battery"); - - if (rmw_uros_epoch_synchronized()) { - msg->header.stamp.sec = (int32_t)(rmw_uros_epoch_nanos() / 1000000000); - msg->header.stamp.nanosec = (uint32_t)(rmw_uros_epoch_nanos() % 1000000000); - } - - msg->power_supply_technology = sensor_msgs__msg__BatteryState__POWER_SUPPLY_TECHNOLOGY_LION; -} - -void fill_wheels_command_msg(std_msgs__msg__Float32MultiArray *msg) { - static float data[MOTORS_COUNT] = {0, 0, 0, 0}; - msg->data.capacity = MOTORS_COUNT; - msg->data.size = 0; - msg->data.data = (float *)data; -} - -void fill_servos_command_msg(std_msgs__msg__UInt32MultiArray *msg) { - static uint32_t data[SERVOS_COUNT] = {0, 0, 0, 0, 0, 0}; - msg->data.capacity = SERVOS_COUNT; - msg->data.size = 0; - msg->data.data = (uint32_t *)data; -} - -void fill_range_msg(sensor_msgs__msg__Range *msg, uint8_t id) { - msg->header.frame_id = micro_ros_string_utilities_set(msg->header.frame_id, range_frame_names[id]); - - if (rmw_uros_epoch_synchronized()) { - msg->header.stamp.sec = (int32_t)(rmw_uros_epoch_nanos() / 1000000000); - msg->header.stamp.nanosec = (uint32_t)(rmw_uros_epoch_nanos() % 1000000000); - } - - msg->radiation_type = sensor_msgs__msg__Range__INFRARED; - msg->field_of_view = 0.26; - msg->min_range = 0.01; - msg->max_range = 0.90; - if(msg->range > msg->max_range || msg->range < msg->min_range){ - msg->range = NAN; - } } \ No newline at end of file diff --git a/src/ranges.cpp b/src/ranges.cpp new file mode 100644 index 0000000..85160be --- /dev/null +++ b/src/ranges.cpp @@ -0,0 +1,42 @@ +#include + +void init_ranges() +{ + MultiDistanceSensor& distance_sensors = MultiDistanceSensor::getInstance(); + if (distance_sensors.init() > 0) + { + uint8_t* data = distance_sensor_commands.alloc(); + *data = 1; + distance_sensor_commands.put(data); + } + + for (auto i = 0u; i < RANGES_COUNT; ++i) + { + fill_range_msg(&range_msgs[i], i); + } +} + +void fill_range_msg(sensor_msgs__msg__Range* msg, uint8_t id) +{ + msg->header.frame_id = micro_ros_string_utilities_set(msg->header.frame_id, range_frame_names[id]); + + if (rmw_uros_epoch_synchronized()) + { + msg->header.stamp.sec = (int32_t)(rmw_uros_epoch_nanos() / 1000000000); + msg->header.stamp.nanosec = (uint32_t)(rmw_uros_epoch_nanos() % 1000000000); + } + + msg->radiation_type = sensor_msgs__msg__Range__INFRARED; + msg->field_of_view = 0.26; + msg->min_range = 0.01; + msg->max_range = 0.90; +} + +void fill_range_msg_with_measurements(sensor_msgs__msg__Range* msg, float range) +{ + msg->range = range; + if (msg->range > msg->max_range || msg->range < msg->min_range) + { + msg->range = NAN; + } +} \ No newline at end of file diff --git a/src/rosbot_sensors.cpp b/src/rosbot_sensors.cpp deleted file mode 100644 index 0ebb9fe..0000000 --- a/src/rosbot_sensors.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#include "rosbot_sensors.h" - -namespace rosbot_sensors{ - -#pragma region BATTERY_REGION - -#define MEASUREMENT_SERIES 10 -#define BATTERY_VOLTAGE_LOW 10.8 - -enum -{ - BATTERY_LOW = 1, - BATTERY_OK = 0 -}; - -typedef struct BatteryData -{ - float voltage; - float threshold; - uint8_t status; -}BatteryData_t; - -static BatteryData_t battery_data = { 0.0, BATTERY_VOLTAGE_LOW, BATTERY_OK}; -static DigitalOut battery_led(LED1,1); -static Ticker battery_led_flipper; -static void readVoltageInternal(); -static AnalogIn battery_adc(BAT_MEAS); - -static void batteryLed() -{ - battery_led = !battery_led; -} - -float updateBatteryWatchdog() -{ - static int index=0; - battery_data.voltage = 3.3f * VIN_MEAS_CORRECTION * (UPPER_RESISTOR + LOWER_RESISTOR)/LOWER_RESISTOR * battery_adc.read(); - if(battery_data.threshold > battery_data.voltage && index < MEASUREMENT_SERIES) // low level - index ++; - else if(battery_data.threshold < battery_data.voltage && index > 0) - index --; - - if(battery_data.status == BATTERY_OK && index == MEASUREMENT_SERIES) - { - battery_data.status = BATTERY_LOW; - battery_led = 0; - battery_led_flipper.attach(callback(batteryLed),0.4); - } - else if(battery_data.status == BATTERY_LOW && index == 0) - { - battery_data.status = BATTERY_OK; - battery_led_flipper.detach(); - battery_led = 1; - } - return battery_data.voltage; -} - -#pragma endregion BATTERY_REGION - -} \ No newline at end of file diff --git a/src/servos.cpp b/src/servos.cpp new file mode 100644 index 0000000..8eb8035 --- /dev/null +++ b/src/servos.cpp @@ -0,0 +1,128 @@ +#include "servos.hpp" +rosbot_sensors::ServoManger servo_manager; +std_msgs__msg__UInt32MultiArray servos_command_msg; + +void init_servos(){ + fill_servos_command_msg(&servos_command_msg); +} + +void fill_servos_command_msg(std_msgs__msg__UInt32MultiArray* msg) +{ + static uint32_t data[SERVOS_COUNT] = { 0, 0, 0, 0, 0, 0 }; + msg->data.capacity = SERVOS_COUNT; + msg->data.size = 0; + msg->data.data = (uint32_t*)data; +} + +void servos_command_callback(const void* message) +{ + const std_msgs__msg__UInt32MultiArray* msg = (const std_msgs__msg__UInt32MultiArray*)message; + if (msg != NULL and msg->data.size == SERVOS_COUNT) + { + for (auto i = 0u; i < SERVOS_COUNT; ++i) + { + servo_manager.setWidth(i, msg->data.data[i]); + } + } +} + +namespace rosbot_sensors +{ + +ServoManger::ServoManger() + : _servo{ nullptr, nullptr, nullptr, nullptr, nullptr, nullptr } + , _voltage_mode(0) + , _enabled_outputs(0) + , _servo_sel1(SERVO_SEL1, 0) + , _servo_sel2(SERVO_SEL2, 0) + , _servo_power(SERVO_POWER_ON, 0) +{ +} + +void ServoManger::enablePower(bool en) +{ + _servo_power.write(en ? 1 : 0); +} + +int ServoManger::getEnabledOutputs() +{ + return _enabled_outputs; +} + +PwmOut* ServoManger::getOutput(int output) +{ + if (output < SERVO_OUTPUT_1 || output > SERVO_OUTPUT_6) + return nullptr; + + return _servo[output]; +} + +bool ServoManger::setWidth(int output, int width_us) +{ + if (output < SERVO_OUTPUT_1 || output > SERVO_OUTPUT_6) + return false; + + if (_servo[output] == nullptr) + return false; + + _servo[output]->pulsewidth_us(width_us); + return true; +} + +bool ServoManger::setPeriod(int output, int period_us) +{ + if (output < SERVO_OUTPUT_1 || output > SERVO_OUTPUT_6) + return false; + + if (_servo[output] == nullptr) + return false; + + _servo[output]->period_us(period_us); + return true; +} + +void ServoManger::enableOutput(int output, bool en) +{ + if (output < SERVO_OUTPUT_1 || output > SERVO_OUTPUT_6) + return; + + if (en && _servo[output] == nullptr) + { + switch (output) + { + case SERVO_OUTPUT_1: + _servo[output] = new PwmOut(SERVO1_PWM); + break; + case SERVO_OUTPUT_2: + _servo[output] = new PwmOut(SERVO2_PWM); + break; + case SERVO_OUTPUT_3: + _servo[output] = new PwmOut(SERVO3_PWM); + break; + case SERVO_OUTPUT_4: + _servo[output] = new PwmOut(SERVO4_PWM); + break; + case SERVO_OUTPUT_5: + _servo[output] = new PwmOut(SERVO5_PWM_ALT1); + break; + case SERVO_OUTPUT_6: + _servo[output] = new PwmOut(SERVO6_PWM_ALT1); + break; + } + _enabled_outputs++; + } + else if (_servo[output] != nullptr && !en) + { + delete _servo[output]; + _servo[output] = nullptr; + _enabled_outputs--; + } +} + +void ServoManger::setPowerMode(int mode) +{ + _servo_sel1.write(mode & 0x01L); + _servo_sel2.write(mode & 0x02L); +} + +} // namespace rosbot_sensors diff --git a/src/wheels.cpp b/src/wheels.cpp new file mode 100644 index 0000000..077bbd4 --- /dev/null +++ b/src/wheels.cpp @@ -0,0 +1,139 @@ +#include "wheels.hpp" + +std_msgs__msg__Float32MultiArray wheels_command_msg; + +void init_wheels() +{ + odom_watchdog_timer.start(); + last_wheels_speed_calc_time = odom_watchdog_timer.read(); + RosbotWheel custom_wheel_params = { .radius = WHEEL_RADIUS, + .diameter_modificator = DIAMETER_MODIFICATOR, + .tyre_deflation = TYRE_DEFLATION, + .gear_ratio = GEAR_RATIO, + .encoder_cpr = ENCODER_CPR, + .polarity = POLARITY }; + + RosbotDrive& drive = RosbotDrive::getInstance(); + drive.setupMotorSequence(MOTOR_FR, MOTOR_FL, MOTOR_RR, MOTOR_RL); + drive.init(custom_wheel_params, RosbotDrive::DEFAULT_REGULATOR_PARAMS); + drive.enable(true); + drive.enablePidReg(true); + + fill_wheels_state_msg(&wheels_state_msg); + fill_wheels_command_msg(&wheels_command_msg); + wheels_state_read_thread.start(wheels_state_read_and_watchdog_task); +} + +void update_wheels_states() +{ + RosbotDrive& drive = RosbotDrive::getInstance(); + + float current_position[MOTORS_COUNT]; + current_position[motor_left_front] = drive.getAngularPos(MOTOR_FL); + current_position[motor_right_front] = drive.getAngularPos(MOTOR_FR); + current_position[motor_left_rear] = drive.getAngularPos(MOTOR_RL); + current_position[motor_right_rear] = drive.getAngularPos(MOTOR_RR); + + const float current_time = odom_watchdog_timer.read(); + const float dt = current_time - last_wheels_speed_calc_time; + last_wheels_speed_calc_time = current_time; + + for (auto i = 0u; i < MOTORS_COUNT; ++i) + { + wheels_state_msg.velocity.data[i] = (current_position[i] - wheels_state_msg.position.data[i]) / dt; + wheels_state_msg.position.data[i] = current_position[i]; + } +} + +void wheels_command_callback(const void* message) +{ + const std_msgs__msg__Float32MultiArray* msg = (const std_msgs__msg__Float32MultiArray*)message; + if (msg != NULL and msg->data.size == MOTORS_COUNT) + { + RosbotDrive& drive = RosbotDrive::getInstance(); + NewTargetSpeed new_speed = { + .speed = { + msg->data.data[motor_right_front] * WHEEL_RADIUS, + msg->data.data[motor_right_rear] * WHEEL_RADIUS, + msg->data.data[motor_left_rear] * WHEEL_RADIUS, + msg->data.data[motor_left_front] * WHEEL_RADIUS, + }, + .mode = MPS + }; + + for (auto i = 0u; i < MOTORS_COUNT; ++i) + { + if (isnan(new_speed.speed[i])) + { + float zero_speeds[MOTORS_COUNT] = { 0, 0, 0, 0 }; + memcpy(new_speed.speed, zero_speeds, sizeof(new_speed.speed)); + break; + } + } + + drive.updateTargetSpeed(new_speed); + last_speed_command_time = odom_watchdog_timer.read_ms(); + is_speed_watchdog_active = false; + } +} + +void check_speed_watchdog() +{ + if (!is_speed_watchdog_active && (odom_watchdog_timer.read_ms() - last_speed_command_time) > speed_watchdog_interval) + { + RosbotDrive& drive = RosbotDrive::getInstance(); + NewTargetSpeed new_speed = { .speed = { 0.0, 0.0, 0.0, 0.0 }, .mode = MPS }; + drive.updateTargetSpeed(new_speed); + is_speed_watchdog_active = true; + } +} + +void wheels_state_read_and_watchdog_task() +{ + while (true) + { + check_speed_watchdog(); + update_wheels_states(); + ThisThread::sleep_for(10); + } +} + +void fill_wheels_state_msg(sensor_msgs__msg__JointState* msg) +{ + static double msg_data_tab[MOTORS_STATE_COUNT][MOTORS_COUNT]; + static rosidl_runtime_c__String msg_name_tab[MOTORS_COUNT]; + msg->header.frame_id = micro_ros_string_utilities_set(msg->header.frame_id, "wheels_state"); + + msg->position.data = msg_data_tab[motor_state_position]; + msg->position.capacity = msg->position.size = MOTORS_COUNT; + msg->velocity.data = msg_data_tab[motor_state_velocity]; + msg->velocity.capacity = msg->velocity.size = MOTORS_COUNT; + msg->effort.data = msg_data_tab[motor_state_effort]; + msg->effort.capacity = msg->effort.size = MOTORS_COUNT; + + msg_name_tab->capacity = msg_name_tab->size = MOTORS_COUNT; + msg_name_tab[motor_left_front].data = (char*)FRONT_LEFT_MOTOR_NAME; + msg_name_tab[motor_right_front].data = (char*)FRONT_RIGHT_MOTOR_NAME; + msg_name_tab[motor_left_rear].data = (char*)REAR_LEFT_MOTOR_NAME; + msg_name_tab[motor_right_rear].data = (char*)REAR_RIGHT_MOTOR_NAME; + for (uint8_t i = 0; i < MOTORS_COUNT; i++) + { + msg_name_tab[i].capacity = msg_name_tab[i].size = strlen(msg_name_tab[i].data); + } + msg->name.capacity = msg->name.size = MOTORS_COUNT; + msg->name.data = msg_name_tab; + + if (rmw_uros_epoch_synchronized()) + { + msg->header.stamp.sec = (int32_t)(rmw_uros_epoch_nanos() / 1000000000); + msg->header.stamp.nanosec = (uint32_t)(rmw_uros_epoch_nanos() % 1000000000); + } +} + +void fill_wheels_command_msg(std_msgs__msg__Float32MultiArray* msg) +{ + static float data[MOTORS_COUNT] = { 0, 0, 0, 0 }; + msg->data.capacity = MOTORS_COUNT; + msg->data.size = 0; + msg->data.data = (float*)data; +} \ No newline at end of file