diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml index d650d044..2e344d14 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml @@ -2,18 +2,18 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - - rmcs_core::hardware::SteeringInfantry -> steeringInfantry_hardware + - rmcs_core::hardware::SteeringInfantry -> infantry_hardware - rmcs_core::referee::Status -> referee_status - - rmcs_core::referee::Command -> referee_command - - rmcs_core::referee::command::Interaction -> referee_interaction - rmcs_core::referee::command::interaction::Ui -> referee_ui - # - rmcs_core::referee::app::ui::Infantry -> referee_ui_infantry + - rmcs_core::referee::app::ui::Infantry -> referee_ui_infantry + - rmcs_core::referee::Command -> referee_command - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller - - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller - rmcs_core::controller::shooting::HeatController -> heat_controller @@ -26,37 +26,47 @@ rmcs_executor: - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller -steeringInfantry_hardware: +infantry_hardware: ros__parameters: - board_serial_top_board: "(TODO)" - board_serial_bottom_board: "(TODO)" - yaw_motor_zero_point: 32285 - pitch_motor_zero_point: 6321 - left_front_zero_point: 7848 - left_back_zero_point: 5770 - right_back_zero_point: 2380 - right_front_zero_point: 1705 + board_serial_top_board: "AF-B4E5-CE0E-4342-FF2C-F9E2-DE47-2D85-9B75" + board_serial_bottom_board: "AF-EEF5-24BA-6675-F1B1-C797-50AF-869C-870E" + yaw_motor_zero_point: 20993 + pitch_motor_zero_point: 18874 + left_front_zero_point: 1695 + right_front_zero_point: 5068 + left_back_zero_point: 7870 + right_back_zero_point: 3141 + gimbal_controller: ros__parameters: - upper_limit: -0.39518 - lower_limit: 0.36 + upper_limit: -0.54 + lower_limit: 0.274 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 15.0 + ki: 0.0 + kd: 1.5 yaw_angle_pid_controller: ros__parameters: measurement: /gimbal/yaw/control_angle_error control: /gimbal/yaw/control_velocity - kp: 16.0 + kp: 20.0 ki: 0.0 - kd: 0.0 + kd: 0.9 -pitch_angle_pid_controller: +yaw_velocity_pid_controller: ros__parameters: - measurement: /gimbal/pitch/control_angle_error - control: /gimbal/pitch/control_velocity - kp: 10.00 - ki: 0.0 - kd: 0.0 + measurement: /gimbal/yaw/velocity_imu + setpoint: /gimbal/yaw/control_velocity + control: /gimbal/yaw/control_torque + kp: 2.5 + ki: 0.00 + kd: 0.6 friction_wheel_controller: ros__parameters: @@ -64,9 +74,9 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 600.0 - - 600.0 - friction_soft_start_stop_time: 1.0 + - 590.0 + - 590.0 + friction_soft_start_stop_time: 0.3 heat_controller: ros__parameters: @@ -75,8 +85,8 @@ heat_controller: bullet_feeder_controller: ros__parameters: - bullets_per_feeder_turn: 10.0 - shot_frequency: 24.0 + bullets_per_feeder_turn: 9.0 + shot_frequency: 27.0 safe_shot_frequency: 10.0 eject_frequency: 15.0 eject_time: 0.15 @@ -86,8 +96,8 @@ bullet_feeder_controller: shooting_recorder: ros__parameters: - friction_wheel_count: 4 - log_mode: 2 # 1: trigger, 2: timing + friction_wheel_count: 2 + log_mode: 1 left_friction_velocity_pid_controller: ros__parameters: @@ -112,15 +122,15 @@ bullet_feeder_velocity_pid_controller: measurement: /gimbal/bullet_feeder/velocity setpoint: /gimbal/bullet_feeder/control_velocity control: /gimbal/bullet_feeder/control_torque - kp: 0.283 + kp: 0.7 ki: 0.0 kd: 0.0 steering_wheel_controller: ros__parameters: - mess: 19.0 - moment_of_inertia: 1.0 - vehicle_radius: 0.24678 + mess: 22.0 + moment_of_inertia: 1.08 + vehicle_radius: 0.28284271247462 wheel_radius: 0.055 friction_coefficient: 0.6 k1: 2.958580e+00 diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index dc9a9805..d9504de2 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -17,8 +17,8 @@ include(FetchContent) set(BUILD_STATIC_LIBRMCS ON CACHE BOOL "Build static librmcs SDK" FORCE) FetchContent_Declare( librmcs - URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.2.0b0/librmcs-sdk-src-3.2.0-beta.0.zip - URL_HASH SHA256=391b642a31473fdb639573912e0fc6266c3819d787b91440310cd1025af1dc3c + URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.2.0/librmcs-sdk-src-3.2.0.zip + URL_HASH SHA256=f81c3af7fbcf35727a8a7586200db8e9bf668ee0f448529de4bfd3eb7c36ed6f DOWNLOAD_EXTRACT_TIMESTAMP TRUE ) FetchContent_MakeAvailable(librmcs) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp index 2b280c19..140e2db1 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp @@ -1,13 +1,15 @@ +#include #include +#include #include #include #include #include #include -#include #include #include +#include #include #include #include @@ -42,7 +44,6 @@ class SteeringInfantry create_partner_component( get_component_name() + "_command", *this)) { register_output("/tf", tf_); - gimbal_calibrate_subscription_ = create_subscription( "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { gimbal_calibrate_subscription_callback(std::move(msg)); @@ -54,7 +55,6 @@ class SteeringInfantry top_board_ = std::make_unique( *this, *command_component_, get_parameter("board_serial_top_board").as_string()); - bottom_board_ = std::make_unique( *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); @@ -75,7 +75,6 @@ class SteeringInfantry } void command_update() { - top_board_->command_update(); bottom_board_->command_update(); } @@ -93,70 +92,61 @@ class SteeringInfantry void steers_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { RCLCPP_INFO( get_logger(), "[steer calibration] New left front offset: %d", - bottom_board_->chassis_steer_motors_[0].calibrate_zero_point()); + bottom_board_->chassis_front_steering_motors_[0].calibrate_zero_point()); RCLCPP_INFO( get_logger(), "[steer calibration] New left back offset: %d", - bottom_board_->chassis_steer_motors_[1].calibrate_zero_point()); + bottom_board_->chassis_back_steering_motors_[0].calibrate_zero_point()); RCLCPP_INFO( get_logger(), "[steer calibration] New right back offset: %d", - bottom_board_->chassis_steer_motors_[2].calibrate_zero_point()); + bottom_board_->chassis_back_steering_motors_[1].calibrate_zero_point()); RCLCPP_INFO( get_logger(), "[steer calibration] New right front offset: %d", - bottom_board_->chassis_steer_motors_[3].calibrate_zero_point()); + bottom_board_->chassis_front_steering_motors_[1].calibrate_zero_point()); } + class SteeringInfantryCommand : public rmcs_executor::Component { public: - explicit SteeringInfantryCommand(SteeringInfantry& steering_infantry) - : steering_infantry(steering_infantry) {} + explicit SteeringInfantryCommand(SteeringInfantry& infantry_) + : infantry_(infantry_) {} - void update() override { steering_infantry.command_update(); } + void update() override { infantry_.command_update(); } - SteeringInfantry& steering_infantry; + SteeringInfantry& infantry_; }; + std::shared_ptr command_component_; - class TopBoard final : private librmcs::agent::CBoard { + class TopBoard final : private librmcs::agent::RmcsBoardLite { public: friend class SteeringInfantry; explicit TopBoard( SteeringInfantry& steering_infantry, SteeringInfantryCommand& steering_infantry_command, std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) + : librmcs::agent::RmcsBoardLite(board_serial, {true}) , tf_(steering_infantry.tf_) - , bmi088_(1000, 0.2, 0.0) + , imu_(1000, 0.2, 0.0) , gimbal_pitch_motor_(steering_infantry, steering_infantry_command, "/gimbal/pitch") , gimbal_left_friction_( steering_infantry, steering_infantry_command, "/gimbal/left_friction") , gimbal_right_friction_( steering_infantry, steering_infantry_command, "/gimbal/right_friction") { - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("pitch_motor_zero_point").as_int()))); + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_encoder_zero_point( + static_cast( + steering_infantry.get_parameter("pitch_motor_zero_point").as_int())) + .enable_multi_turn_angle()); gimbal_left_friction_.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); gimbal_right_friction_.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reduction_ratio(1.) .set_reversed()); - - steering_infantry.register_output( - "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + steering_infantry.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); steering_infantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - - bmi088_.set_coordinate_mapping([](double x, double y, double z) { - // Get the mapping with the following code. - // The rotation angle must be an exact multiple of 90 degrees, otherwise - // use a matrix. - - // Eigen::AngleAxisd pitch_link_to_bmi088_link{ - // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; - // Eigen::Vector3d mapping = pitch_link_to_bmi088_link * Eigen::Vector3d{1, 2, 3}; - // std::cout << mapping << std::endl; - - return std::make_tuple(x, y, z); - }); + "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + imu_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(x, y, z); }); } TopBoard(const TopBoard&) = delete; @@ -164,18 +154,17 @@ class SteeringInfantry TopBoard(TopBoard&&) = delete; TopBoard& operator=(TopBoard&&) = delete; - ~TopBoard() override = default; + ~TopBoard() final = default; void update() { - bmi088_.update_status(); - const Eigen::Quaterniond gimbal_bmi088_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + imu_.update_status(); + const Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; tf_->set_transform( - gimbal_bmi088_pose.conjugate()); + gimbal_imu_pose.conjugate()); - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - *gimbal_pitch_velocity_bmi088_ = bmi088_.gy(); + *gimbal_yaw_velocity_imu_ = imu_.gz(); + *gimbal_pitch_velocity_imu_ = imu_.gy(); gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); @@ -192,19 +181,17 @@ class SteeringInfantry .can_id = 0x200, .can_data = device::CanPacket8{ + gimbal_right_friction_.generate_command(), + gimbal_left_friction_.generate_command(), device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), } .as_bytes(), }); builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_pitch_motor_ - .generate_velocity_command(gimbal_pitch_motor_.control_velocity()) - .as_bytes(), + .can_id = 0x143, + .can_data = gimbal_pitch_motor_.generate_velocity_command().as_bytes(), }); } @@ -213,120 +200,131 @@ class SteeringInfantry if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; auto can_id = data.can_id; - if (can_id == 0x203) { - gimbal_left_friction_.store_status(data.can_data); - } else if (can_id == 0x204) { + if (can_id == 0x201) { gimbal_right_friction_.store_status(data.can_data); + } else if (can_id == 0x202) { + gimbal_left_friction_.store_status(data.can_data); } } + void can2_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; auto can_id = data.can_id; - if (can_id == 0x142) + if (can_id == 0x143) { gimbal_pitch_motor_.store_status(data.can_data); + } } + void accelerometer_receive_callback( const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); + imu_.store_accelerometer_status(data.x, data.y, data.z); } + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); + imu_.store_gyroscope_status(data.x, data.y, data.z); } + OutputInterface& tf_; - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_bmi088_; + device::Bmi088 imu_; - device::Bmi088 bmi088_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; }; - class BottomBoard final : private librmcs::agent::CBoard { + class BottomBoard final : private librmcs::agent::RmcsBoardLite { public: friend class SteeringInfantry; - explicit BottomBoard( SteeringInfantry& steering_infantry, SteeringInfantryCommand& steering_infantry_command, std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , imu_(1000, 0.2, 0.0) + : librmcs::agent::RmcsBoardLite(board_serial) , tf_(steering_infantry.tf_) + , imu_(1000, 0.2, 0.0) , dr16_(steering_infantry) , gimbal_yaw_motor_(steering_infantry, steering_infantry_command, "/gimbal/yaw") + , supercap_(steering_infantry, steering_infantry_command) , gimbal_bullet_feeder_( steering_infantry, steering_infantry_command, "/gimbal/bullet_feeder") - , chassis_wheel_motors_( + , chassis_front_steering_motors_( + {steering_infantry, steering_infantry_command, "/chassis/left_front_steering"}, + {steering_infantry, steering_infantry_command, "/chassis/right_front_steering"}) + , chassis_front_wheel_motors_( {steering_infantry, steering_infantry_command, "/chassis/left_front_wheel"}, - {steering_infantry, steering_infantry_command, "/chassis/left_back_wheel"}, - {steering_infantry, steering_infantry_command, "/chassis/right_back_wheel"}, {steering_infantry, steering_infantry_command, "/chassis/right_front_wheel"}) - , chassis_steer_motors_( - {steering_infantry, steering_infantry_command, "/chassis/left_front_steering"}, + , chassis_back_steering_motors_( {steering_infantry, steering_infantry_command, "/chassis/left_back_steering"}, - {steering_infantry, steering_infantry_command, "/chassis/right_back_steering"}, - {steering_infantry, steering_infantry_command, "/chassis/right_front_steering"}) - , supercap_(steering_infantry, steering_infantry_command) { - - steering_infantry.register_output("/referee/serial", referee_serial_); - - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart1_transmit( - {.uart_data = std::span{buffer, size}}); - return size; - }; + {steering_infantry, steering_infantry_command, "/chassis/right_back_steering"}) + , chassis_back_wheel_motors_( + {steering_infantry, steering_infantry_command, "/chassis/left_back_wheel"}, + {steering_infantry, steering_infantry_command, "/chassis/right_back_wheel"}) { gimbal_yaw_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( static_cast( steering_infantry.get_parameter("yaw_motor_zero_point").as_int()))); - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006} - .enable_multi_turn_angle() + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() - .set_reduction_ratio(19 * 2)); - - for (auto& motor : chassis_wheel_motors_) - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .enable_multi_turn_angle()); - .set_reduction_ratio(11.) - .enable_multi_turn_angle() - .set_reversed()); - chassis_steer_motors_[0].configure( + chassis_front_steering_motors_[0].configure( device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() .set_encoder_zero_point( static_cast( steering_infantry.get_parameter("left_front_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[1].configure( + .set_reversed()); + chassis_front_steering_motors_[1].configure( device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() .set_encoder_zero_point( static_cast( - steering_infantry.get_parameter("left_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[2].configure( + steering_infantry.get_parameter("right_front_zero_point").as_int())) + .set_reversed()); + chassis_back_steering_motors_[0].configure( device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() .set_encoder_zero_point( static_cast( - steering_infantry.get_parameter("right_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[3].configure( + steering_infantry.get_parameter("left_back_zero_point").as_int())) + .set_reversed()); + chassis_back_steering_motors_[1].configure( device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() .set_encoder_zero_point( static_cast( - steering_infantry.get_parameter("right_front_zero_point").as_int())) - .enable_multi_turn_angle()); + steering_infantry.get_parameter("right_back_zero_point").as_int())) + .set_reversed()); + + chassis_front_wheel_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_front_wheel_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_back_wheel_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_back_wheel_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + + steering_infantry.register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + start_transmit().uart0_transmit( + {.uart_data = std::span{buffer, size}}); + return size; + }; + steering_infantry.register_output( "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); } @@ -336,162 +334,165 @@ class SteeringInfantry BottomBoard(BottomBoard&&) = delete; BottomBoard& operator=(BottomBoard&&) = delete; - ~BottomBoard() override = default; + ~BottomBoard() final = default; void update() { imu_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gy(); - supercap_.update_status(); + dr16_.update_status(); - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steer_motors_) - motor.update_status(); + *chassis_yaw_velocity_imu_ = imu_.gz(); - dr16_.update_status(); gimbal_yaw_motor_.update_status(); + supercap_.update_status(); + gimbal_bullet_feeder_.update_status(); + tf_->set_state( gimbal_yaw_motor_.angle()); - gimbal_bullet_feeder_.update_status(); + for (auto& motor : chassis_front_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_front_steering_motors_) + motor.update_status(); + for (auto& motor : chassis_back_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_back_steering_motors_) + motor.update_status(); } void command_update() { - if (can_transmission_mode_) { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FF, - .can_data = - device::CanPacket8{ - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); - - auto steer_builder = start_transmit(); - steer_builder.can2_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[0].generate_command(), - chassis_steer_motors_[1].generate_command(), - chassis_steer_motors_[2].generate_command(), - chassis_steer_motors_[3].generate_command(), - } - .as_bytes(), - }); - } else { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FF, - .can_data = - device::CanPacket8{ - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_yaw_motor_ - .generate_velocity_command( - gimbal_yaw_motor_.control_velocity() - imu_.gy()) - .as_bytes(), - }); - } - can_transmission_mode_ = !can_transmission_mode_; + auto builder = start_transmit(); + builder.can0_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_back_wheel_motors_[0].generate_command(), + chassis_back_wheel_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can0_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_back_steering_motors_[0].generate_command(), + chassis_back_steering_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_front_wheel_motors_[0].generate_command(), + chassis_front_wheel_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_front_steering_motors_[0].generate_command(), + chassis_front_steering_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + + } + .as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x142, + .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x1FF, + .can_data = + device::CanPacket8{ + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); } private: - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + + if (can_id == 0x201) { + chassis_back_wheel_motors_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_back_wheel_motors_[1].store_status(data.can_data); + } else if (can_id == 0x205) { + chassis_back_steering_motors_[0].store_status(data.can_data); + } else if (can_id == 0x206) { + chassis_back_steering_motors_[1].store_status(data.can_data); + } else if (can_id == 0x300) { + supercap_.store_status(data.can_data); + } } void can1_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; auto can_id = data.can_id; - if (can_id == 0x201) - chassis_wheel_motors_[0].store_status(data.can_data); - else if (can_id == 0x202) - chassis_wheel_motors_[1].store_status(data.can_data); - else if (can_id == 0x203) - chassis_wheel_motors_[2].store_status(data.can_data); - else if (can_id == 0x204) - chassis_wheel_motors_[3].store_status(data.can_data); - else if (can_id == 0x205) - gimbal_bullet_feeder_.store_status(data.can_data); - else if (can_id == 0x300) - supercap_.store_status(data.can_data); + + if (can_id == 0x201) { + chassis_front_wheel_motors_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_front_wheel_motors_[1].store_status(data.can_data); + } else if (can_id == 0x205) { + chassis_front_steering_motors_[0].store_status(data.can_data); + } else if (can_id == 0x206) { + chassis_front_steering_motors_[1].store_status(data.can_data); + } } void can2_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; auto can_id = data.can_id; - if (can_id == 0x142) + if (can_id == 0x142) { gimbal_yaw_motor_.store_status(data.can_data); - else if (can_id == 0x205) - chassis_steer_motors_[0].store_status(data.can_data); - else if (can_id == 0x206) - chassis_steer_motors_[1].store_status(data.can_data); - else if (can_id == 0x207) - chassis_steer_motors_[2].store_status(data.can_data); - else if (can_id == 0x208) - chassis_steer_motors_[3].store_status(data.can_data); + } + } + + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + + if (can_id == 0x205) { + gimbal_bullet_feeder_.store_status(data.can_data); + } } - void uart1_receive_callback(const librmcs::data::UartDataView& data) override { + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { const auto* uart_data = data.uart_data.data(); referee_ring_buffer_receive_.emplace_back_n( [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, data.uart_data.size()); } + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + void accelerometer_receive_callback( const librmcs::data::AccelerometerDataView& data) override { imu_.store_accelerometer_status(data.x, data.y, data.z); @@ -501,32 +502,34 @@ class SteeringInfantry imu_.store_gyroscope_status(data.x, data.y, data.z); } - bool can_transmission_mode_ = true; - device::Bmi088 imu_; OutputInterface& tf_; - OutputInterface powermeter_control_enabled_; - OutputInterface powermeter_charge_power_limit_; + device::Bmi088 imu_; device::Dr16 dr16_; + device::LkMotor gimbal_yaw_motor_; - device::DjiMotor gimbal_bullet_feeder_; - device::DjiMotor chassis_wheel_motors_[4]; - device::DjiMotor chassis_steer_motors_[4]; + device::Supercap supercap_; + device::DjiMotor gimbal_bullet_feeder_; + + device::DjiMotor chassis_front_steering_motors_[2]; + device::DjiMotor chassis_front_wheel_motors_[2]; + device::DjiMotor chassis_back_steering_motors_[2]; + device::DjiMotor chassis_back_wheel_motors_[2]; rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; OutputInterface referee_serial_; + OutputInterface chassis_yaw_velocity_imu_; }; OutputInterface tf_; - std::shared_ptr command_component_; - std::shared_ptr top_board_; - std::shared_ptr bottom_board_; - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; rclcpp::Subscription::SharedPtr steers_calibrate_subscription_; + + std::shared_ptr top_board_; + std::shared_ptr bottom_board_; }; } // namespace rmcs_core::hardware