diff --git a/include/solo/solo12.hpp b/include/solo/solo12.hpp index 59a67cf..3062d01 100644 --- a/include/solo/solo12.hpp +++ b/include/solo/solo12.hpp @@ -297,6 +297,39 @@ class Solo12 return imu_linear_acceleration_; } + /** + * @brief Get current [A] measurement from the power board. + * + * Will be zero if no power board is used. + */ + float get_powerboard_current() const + { + // FIXME better get in acquire_sensors() so its in sync with other + // sensor data + return main_board_ptr_->powerboard_current(); + } + + /** + * @brief Get voltage [V] measurement from the power board. + * + * Will be zero if no power board is used. + */ + float get_powerboard_voltage() const + { + return main_board_ptr_->powerboard_voltage(); + } + + /** + * @brief Get energy [J] consumed since start as measured by the power + * board. + * + * Will be zero if no power board is used. + */ + float get_powerboard_energy() const + { + return main_board_ptr_->powerboard_energy(); + } + /* * Hardware Status */ diff --git a/src/dynamic_graph_manager/dgm_solo12.cpp b/src/dynamic_graph_manager/dgm_solo12.cpp index 26a5f6f..174da9d 100644 --- a/src/dynamic_graph_manager/dgm_solo12.cpp +++ b/src/dynamic_graph_manager/dgm_solo12.cpp @@ -121,6 +121,9 @@ void DGMSolo12::get_sensors_to_map(dynamic_graph_manager::VectorDGMap& map) map.at("imu_gyroscope") = solo_.get_imu_gyroscope(); map.at("imu_attitude") = solo_.get_imu_attitude(); map.at("imu_linear_acceleration") = solo_.get_imu_linear_acceleration(); + map.at("powerboard_current")[0] = solo_.get_powerboard_current(); + map.at("powerboard_voltage")[0] = solo_.get_powerboard_voltage(); + map.at("powerboard_energy")[0] = solo_.get_powerboard_energy(); /** * Robot status. diff --git a/src/solo12.cpp b/src/solo12.cpp index 6c3acbb..44f18f6 100644 --- a/src/solo12.cpp +++ b/src/solo12.cpp @@ -1,8 +1,8 @@ #include "solo/solo12.hpp" #include #include -#include "solo/common_programs_header.hpp" #include "real_time_tools/spinner.hpp" +#include "solo/common_programs_header.hpp" namespace solo { @@ -79,8 +79,7 @@ void Solo12::initialize(const std::string& network_id, network_id_ = network_id; // Use a serial port to read slider values. - serial_reader_ = - std::make_shared(serial_port, 5); + serial_reader_ = std::make_shared(serial_port, 5); main_board_ptr_ = std::make_shared(network_id_); @@ -141,9 +140,12 @@ void Solo12::initialize(const std::string& network_id, calib_ctrl_ = std::make_shared( joints_, directions, position_offsets, 5., 0.05, 1.0, 0.001); + auto powerboard = + std::make_shared(main_board_ptr_); + // Define the robot. robot_ = std::make_shared( - main_board_ptr_, joints_, imu_, calib_ctrl_); + main_board_ptr_, joints_, imu_, calib_ctrl_, powerboard); // Initialize the robot. robot_->Init(); @@ -279,7 +281,7 @@ void Solo12::send_target_joint_position( } void Solo12::send_target_joint_velocity( - const Eigen::Ref target_joint_velocity) + const Eigen::Ref target_joint_velocity) { robot_->joints->SetDesiredVelocities(target_joint_velocity); } @@ -301,7 +303,7 @@ void Solo12::wait_until_ready() real_time_tools::Spinner spinner; spinner.set_period(0.001); static long int count_wait_until_ready = 0; - while(state_ != Solo12State::ready) + while (state_ != Solo12State::ready) { if (count_wait_until_ready % 200 == 0) {