diff --git a/include/rm_manual/legged_wheel_balance_manual.h b/include/rm_manual/legged_wheel_balance_manual.h new file mode 100644 index 00000000..67c41156 --- /dev/null +++ b/include/rm_manual/legged_wheel_balance_manual.h @@ -0,0 +1,37 @@ +// +// Created by kook on 9/28/24. +// + +#pragma once + +#include "rm_manual/balance_manual.h" + +namespace rm_manual +{ +class LeggedWheelBalanceManual : public BalanceManual +{ +public: + LeggedWheelBalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee); + +protected: + void updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) override; + void shiftPress() override; + void shiftRelease() override; + void bPress() override; + void bRelease() override; + void ctrlZPress() override; + void rightSwitchDownRise() override; + void rightSwitchMidRise() override; + void ctrlShiftPress(); + void ctrlShiftRelease(); + + void sendCommand(const ros::Time& time) override; + void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override; + void ctrlGPress(); + rm_common::LegCommandSender* legCommandSender_{}; + +private: + bool stretch_ = false; + InputEvent ctrl_shift_event_, ctrl_g_event_; +}; +} // namespace rm_manual diff --git a/include/rm_manual/wheeled_balance_manual.h b/include/rm_manual/wheeled_balance_manual.h index 744ffb10..c3a04322 100644 --- a/include/rm_manual/wheeled_balance_manual.h +++ b/include/rm_manual/wheeled_balance_manual.h @@ -32,6 +32,6 @@ class WheeledBalanceManual : public BalanceManual ros::Subscriber state_sub_; double balance_dangerous_angle_; - InputEvent v_event_, ctrl_x_event_, auto_fallen_event_; + InputEvent ctrl_x_event_, auto_fallen_event_; }; } // namespace rm_manual diff --git a/src/balance_manual.cpp b/src/balance_manual.cpp index e1a8e548..386f2700 100644 --- a/src/balance_manual.cpp +++ b/src/balance_manual.cpp @@ -9,8 +9,9 @@ namespace rm_manual BalanceManual::BalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) : ChassisGimbalShooterCoverManual(nh, nh_referee) { - nh.param("flank_frame", flank_frame_, std::string("flank_frame")); - nh.param("reverse_frame", reverse_frame_, std::string("yaw_reverse_frame")); + ros::NodeHandle balance_nh(nh, "balance"); + balance_nh.param("flank_frame", flank_frame_, std::string("flank_frame")); + balance_nh.param("reverse_frame", reverse_frame_, std::string("yaw_reverse_frame")); is_balance_ = true; } @@ -91,7 +92,7 @@ void BalanceManual::shiftPress() { ChassisGimbalShooterCoverManual::shiftPress(); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::UP_SLOPE); - chassis_cmd_sender_->updateSafetyPower(60); + chassis_cmd_sender_->updateSafetyPower(220); } void BalanceManual::wPress() diff --git a/src/legged_wheel_balance_manual.cpp b/src/legged_wheel_balance_manual.cpp new file mode 100644 index 00000000..bac55aef --- /dev/null +++ b/src/legged_wheel_balance_manual.cpp @@ -0,0 +1,120 @@ +// +// Created by kook on 9/28/24. +// + +#include "rm_manual/legged_wheel_balance_manual.h" + +namespace rm_manual +{ +LeggedWheelBalanceManual::LeggedWheelBalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) + : BalanceManual(nh, nh_referee) +{ + ros::NodeHandle leg_nh(nh, "balance/legged_wheel_chassis"); + legCommandSender_ = new rm_common::LegCommandSender(leg_nh); + legCommandSender_->setLgeLength(0.18); + legCommandSender_->setJump(false); + + b_event_.setEdge(boost::bind(&LeggedWheelBalanceManual::bPress, this), + boost::bind(&LeggedWheelBalanceManual::bRelease, this)); + ctrl_shift_event_.setEdge(boost::bind(&LeggedWheelBalanceManual::ctrlShiftPress, this), + boost::bind(&LeggedWheelBalanceManual::ctrlShiftRelease, this)); + ctrl_g_event_.setRising(boost::bind(&LeggedWheelBalanceManual::ctrlGPress, this)); +} + +void LeggedWheelBalanceManual::sendCommand(const ros::Time& time) +{ + BalanceManual::sendCommand(time); + legCommandSender_->sendCommand(time); +} + +void LeggedWheelBalanceManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) +{ + ChassisGimbalShooterCoverManual::checkKeyboard(dbus_data); + ctrl_g_event_.update(dbus_data->key_ctrl && dbus_data->key_g); + ctrl_shift_event_.update(dbus_data->key_ctrl && dbus_data->key_shift); +} + +void LeggedWheelBalanceManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) +{ + BalanceManual::updateRc(dbus_data); + if (!is_gyro_) + { // Capacitor enter fast charge when chassis stop. + if (!dbus_data->wheel && chassis_cmd_sender_->getMsg()->mode == rm_msgs::ChassisCmd::FOLLOW && + std::sqrt(std::pow(vel_cmd_sender_->getMsg()->linear.x, 2) + std::pow(vel_cmd_sender_->getMsg()->linear.y, 2)) > + 0.0) + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST); + else if (chassis_power_ < 6.0 && chassis_cmd_sender_->getMsg()->mode == rm_msgs::ChassisCmd::FOLLOW) + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST); + } + else + { + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); + } +} + +void LeggedWheelBalanceManual::rightSwitchDownRise() +{ + BalanceManual::rightSwitchDownRise(); +} + +void LeggedWheelBalanceManual::rightSwitchMidRise() +{ + BalanceManual::rightSwitchMidRise(); +} + +void LeggedWheelBalanceManual::ctrlZPress() +{ + BalanceManual::ctrlZPress(); + if (!supply_) + { + is_gyro_ = false; + legCommandSender_->setLgeLength(0.18); + } +} + +void LeggedWheelBalanceManual::shiftRelease() +{ + BalanceManual::shiftRelease(); +} + +void LeggedWheelBalanceManual::shiftPress() +{ + BalanceManual::shiftPress(); +} + +void LeggedWheelBalanceManual::ctrlShiftPress() +{ + legCommandSender_->setJump(true); +} + +void LeggedWheelBalanceManual::ctrlShiftRelease() +{ + legCommandSender_->setJump(false); +} + +void LeggedWheelBalanceManual::bPress() +{ + ChassisGimbalShooterCoverManual::bPress(); + chassis_cmd_sender_->updateSafetyPower(60); +} + +void LeggedWheelBalanceManual::bRelease() +{ + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST); + chassis_cmd_sender_->updateSafetyPower(60); +} + +void LeggedWheelBalanceManual::ctrlGPress() +{ + if (!stretch_) + { + legCommandSender_->setLgeLength(0.25); + stretch_ = true; + } + else + { + legCommandSender_->setLgeLength(0.18); + stretch_ = false; + } +} +} // namespace rm_manual diff --git a/src/main.cpp b/src/main.cpp index 28fc4fa8..f813ec22 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -7,6 +7,7 @@ #include "rm_manual/engineer_manual.h" #include "rm_manual/dart_manual.h" #include "rm_manual/wheeled_balance_manual.h" +#include "rm_manual/legged_wheel_balance_manual.h" int main(int argc, char** argv) { @@ -24,8 +25,10 @@ int main(int argc, char** argv) manual_control = new rm_manual::EngineerManual(nh, nh_referee); else if (robot == "dart") manual_control = new rm_manual::DartManual(nh, nh_referee); - else if (robot == "balance") - manual_control = new rm_manual::BalanceManual(nh, nh_referee); + else if (robot == "wheeled_balance") + manual_control = new rm_manual::WheeledBalanceManual(nh, nh_referee); + else if (robot == "legged_wheel_balance") + manual_control = new rm_manual::LeggedWheelBalanceManual(nh, nh_referee); else { ROS_ERROR("no robot type "); diff --git a/src/wheeled_balance_manual.cpp b/src/wheeled_balance_manual.cpp index 31c046b5..a5d8c4c6 100644 --- a/src/wheeled_balance_manual.cpp +++ b/src/wheeled_balance_manual.cpp @@ -9,11 +9,11 @@ namespace rm_manual WheeledBalanceManual::WheeledBalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) : BalanceManual(nh, nh_referee) { - ros::NodeHandle balance_chassis_nh(nh, "balance_chassis"); + ros::NodeHandle balance_chassis_nh(nh, "balance/wheeled_chassis"); balance_chassis_cmd_sender_ = new rm_common::BalanceCommandSender(balance_chassis_nh); balance_chassis_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL); - nh.param("balance_dangerous_angle", balance_dangerous_angle_, 0.3); + balance_chassis_nh.param("balance_dangerous_angle", balance_dangerous_angle_, 0.3); state_sub_ = balance_chassis_nh.subscribe("/state", 1, &WheeledBalanceManual::balanceStateCallback, this);