cxh/sweeper_e800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Auto commit at 2025-06-19 14:14:52

Browse Source
This commit is contained in:
cxh 2025-06-19 14:14:52 +08:00
parent d37c19a471
commit e15bcea458
3 changed files with 41 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/mc/include/mc/dumper.h
View File

@ -5,6 +5,7 @@
#include "can_driver.h"
#include <mutex>
#include <queue>
#include <rclcpp/rclcpp.hpp>
enum class DumperState
{
@ -56,6 +57,7 @@ private:
std::atomic<bool> sweeping_ = false; // 扫地任务标志
DumperState current_state_;
CANDriver &can_;
rclcpp::Logger logger_;
};
#endif

27
src/mc/src/dumper.cpp
View File

@ -7,14 +7,15 @@ DumperController::DumperController(CANDriver &driver)
: busy_(false),
has_pending_task_(false),
current_state_(DumperState::IDLE),
can_(driver)
can_(driver),
logger_(rclcpp::get_logger("DumperController"))
{
}
// 上电复位：自动收斗 + 降斗
void DumperController::onStartup()
{
std::cout << "[Init] Power-on reset: retract and lower...\n";
RCLCPP_INFO(logger_, "[Init] Power-on reset: retract and lower...");
doAction(DumperAction::RETRACT);
doAction(DumperAction::LOWER);
in_startup_reset_ = false;
@ -26,7 +27,7 @@ void DumperController::tryStartup()
if (awakened_)
return;
std::cout << "[Init] Dumper first wake-up: resetting...\n";
RCLCPP_INFO(logger_, "[Init] Dumper first wake-up: resetting...");
onStartup();
awakened_ = true;
}
@ -35,14 +36,14 @@ void DumperController::resetAwake()
{
awakened_ = false;
last_dial_target_ = -1;
std::cout << "[INFO] Dumper awake state reset.\n";
RCLCPP_INFO(logger_, "[INFO] Dumper awake state reset.");
}
void DumperController::updateDial(int new_dial, int current_state)
{
if (in_startup_reset_)
{
std::cout << "[Dumper] In startup reset, ignoring remote control dump command.\n";
RCLCPP_WARN(logger_, "[Dumper] In startup reset, ignoring remote control dump command.");
return;
}
@ -52,7 +53,7 @@ void DumperController::updateDial(int new_dial, int current_state)
if (last_dial_target_ == new_dial)
return;
std::cout << "[Dial] Changed from " << current_state << " to " << new_dial << "\n";
RCLCPP_INFO(logger_, "[Dial] Changed from %d to %d", current_state, new_dial);
handleDialChange(current_state, new_dial);
last_dial_target_ = new_dial;
}
@ -126,7 +127,7 @@ void DumperController::doAction(DumperAction act)
{
if (sweeping_)
{
std::cout << "[Warning] Sweeping active, dumper action blocked.\n";
RCLCPP_WARN(logger_, "[Warning] Sweeping active, dumper action blocked.");
return;
}
@ -141,7 +142,7 @@ void DumperController::doAction(DumperAction act)
case DumperAction::RAISE:
if (raised_)
return;
std::cout << "[Action] Raising...\n";
RCLCPP_INFO(logger_, "[Action] Raising...");
frame1 = {0x18FA1117, {0x00, 0x40, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00}, 8, true, false};
can_.sendFrame(frame1);
current_state_ = DumperState::RAISING;
@ -153,7 +154,7 @@ void DumperController::doAction(DumperAction act)
case DumperAction::DUMP:
if (dumped_)
return;
std::cout << "[Action] Dumping...\n";
RCLCPP_INFO(logger_, "[Action] Dumping...");
frame1 = {0x18FA1117, {0x00, 0x00, 0x00, 0x10, 0x20, 0x00, 0x00, 0x00}, 8, true, false};
can_.sendFrame(frame1);
current_state_ = DumperState::DUMPING;
@ -164,7 +165,7 @@ void DumperController::doAction(DumperAction act)
case DumperAction::RETRACT:
if (!dumped_)
return;
std::cout << "[Action] Retracting...\n";
RCLCPP_INFO(logger_, "[Action] Retracting...");
frame1 = {0x18FA1117, {0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00}, 8, true, false};
frame2 = {0x18F21117, {0x00, 0x0A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 8, true, false};
can_.sendFrame(frame1);
@ -177,7 +178,7 @@ void DumperController::doAction(DumperAction act)
case DumperAction::LOWER:
if (!raised_)
return;
std::cout << "[Action] Lowering...\n";
RCLCPP_INFO(logger_, "[Action] Lowering...");
frame1 = {0x18FA1117, {0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00}, 8, true, false};
frame2 = {0x18F21117, {0x00, 0x00, 0xA0, 0x00, 0x00, 0x00, 0x00, 0x00}, 8, true, false};
can_.sendFrame(frame1);
@ -197,13 +198,13 @@ void DumperController::sendControlOff()
CANFrame off2 = {0x18F21117, {0, 0, 0, 0, 0, 0, 0, 0}, 8, true, false};
can_.sendFrame(off1);
can_.sendFrame(off2);
std::cout << "[INFO] Control OFF sent.\n";
RCLCPP_INFO(logger_, "[INFO] Control OFF sent.");
}
void DumperController::logState() const
{
const char *state_str[] = {"IDLE", "RAISING", "DUMPING", "RETRACTING", "LOWERING"};
std::cout << "[STATE] Dumper state: " << state_str[static_cast<int>(current_state_)] << "\n";
RCLCPP_INFO(logger_, "[STATE] Dumper state: %s", state_str[static_cast<int>(current_state_)]);
}
int DumperController::getSimpleState() const

51
src/radio_ctrl/src/radio_ctrl.cpp
View File

@ -75,15 +75,14 @@ private:
for (int i = 0; i < 10; ++i)
{
ch_data[i] = uart_handler_->get_channel_value(i);
printf("ch[%d]:%d ", i, ch_data[i]);
// printf("ch[%d]:%d ", i, ch_data[i]);
}
printf("\n");
// printf("\n");
uint16_t ctrl = ch_data[4]; // 手刹
uint16_t gear = ch_data[5]; // 挡位
uint16_t sweep = ch_data[6]; // 清扫
uint16_t dump = ch_data[7]; // 垃圾倾倒
printf("dump=%d\n", dump);
uint16_t ctrl = ch_data[4]; // 手刹
uint16_t gear = ch_data[5]; // 挡位
uint16_t sweep = ch_data[6]; // 清扫
uint16_t dump = ch_data[7]; // 垃圾倾倒
int16_t speed = ch_data[1] - 992; //[-800,800]
if (!initialized)
@ -214,25 +213,25 @@ private:
// 发布控制消息
pub_->publish(msg);
// RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:"
// << "\n brake: " << static_cast<int>(msg.brake) // uint8
// << "\n gear: " << static_cast<int>(msg.gear) // uint8
// << "\n rpm: " << static_cast<int>(msg.rpm) // uint8
// << "\n ehb_enable: " << static_cast<int>(msg.ehb_anable) // bool
// << "\n ehb_brake_pressure: " << static_cast<float>(msg.ehb_brake_pressure) // float32
// << "\n angle: " << msg.angle // float32
// << "\n angle_speed: " << msg.angle_speed // uint16
// << "\n edge_brush_lift: " << static_cast<int>(msg.edge_brush_lift) // bool
// << "\n sweep_ctrl: " << static_cast<int>(msg.sweep_ctrl) // bool
// << "\n spray: " << static_cast<int>(msg.spray) // bool
// << "\n mud_flap: " << static_cast<int>(msg.mud_flap) // bool
// << "\n dust_shake: " << static_cast<int>(msg.dust_shake) // bool
// << "\n left_light: " << static_cast<int>(msg.left_light) // bool
// << "\n right_light: " << static_cast<int>(msg.right_light) // bool
// << "\n brake_light: " << static_cast<int>(msg.brake_light) // bool
// << "\n headlight: " << static_cast<int>(msg.headlight) // bool
// << "\n dump: " << static_cast<int>(msg.dump) // uint8
// );
RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:"
<< "\n brake: " << static_cast<int>(msg.brake) // uint8
<< "\n gear: " << static_cast<int>(msg.gear) // uint8
<< "\n rpm: " << static_cast<int>(msg.rpm) // uint8
<< "\n ehb_enable: " << static_cast<int>(msg.ehb_anable) // bool
<< "\n ehb_brake_pressure: " << static_cast<float>(msg.ehb_brake_pressure) // float32
<< "\n angle: " << msg.angle // float32
<< "\n angle_speed: " << msg.angle_speed // uint16
<< "\n edge_brush_lift: " << static_cast<int>(msg.edge_brush_lift) // bool
<< "\n sweep_ctrl: " << static_cast<int>(msg.sweep_ctrl) // bool
<< "\n spray: " << static_cast<int>(msg.spray) // bool
<< "\n mud_flap: " << static_cast<int>(msg.mud_flap) // bool
<< "\n dust_shake: " << static_cast<int>(msg.dust_shake) // bool
<< "\n left_light: " << static_cast<int>(msg.left_light) // bool
<< "\n right_light: " << static_cast<int>(msg.right_light) // bool
<< "\n brake_light: " << static_cast<int>(msg.brake_light) // bool
<< "\n headlight: " << static_cast<int>(msg.headlight) // bool
<< "\n dump: " << static_cast<int>(msg.dump) // uint8
);
}
else
{