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

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

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

43
src/radio_ctrl/src/radio_ctrl.cpp
View File

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