117 lines
4.0 KiB
C++
117 lines
4.0 KiB
C++
#pragma once
|
|
|
|
#include "fgc/IMotorController.h"
|
|
#include "fgc/Logger.h"
|
|
|
|
#include <mutex>
|
|
#include <sstream>
|
|
#include <string>
|
|
|
|
namespace fgc {
|
|
|
|
// Simulated motor controller for development without hardware. Models the new
|
|
// firmware's encoder-count protocol: HOME makes both axes READY; MOVE sets a
|
|
// per-axis target; each telemetry() read steps the encoder toward its target
|
|
// and reports standstill once reached - so the capture scheduler's
|
|
// move -> settle -> trigger loop runs exactly as it would against real hardware.
|
|
class MockMotorController : public IMotorController {
|
|
public:
|
|
void start() override { LOG_INFO << "[mock] motor controller started"; }
|
|
void stop() override { LOG_INFO << "[mock] motor controller stopped"; }
|
|
|
|
void sendCommand(const std::string& cmd) override {
|
|
LOG_TRACE_CAT(LogCat::Serial) << "TX " << cmd;
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
std::istringstream in(cmd);
|
|
std::string verb;
|
|
in >> verb;
|
|
for (auto& c : verb) c = static_cast<char>(std::toupper(static_cast<unsigned char>(c)));
|
|
|
|
if (verb == "HOME") {
|
|
homed_ = true;
|
|
} else if (verb == "MOVE") {
|
|
std::string a;
|
|
in >> a;
|
|
auto comma = a.find(',');
|
|
if (comma != std::string::npos) { // MOVE <yaw>,<pitch>
|
|
tryParse(a.substr(0, comma), yaw_target_);
|
|
tryParse(a.substr(comma + 1), pitch_target_);
|
|
} else { // MOVE Y|P <pos>
|
|
long pos = 0;
|
|
if (in >> pos) {
|
|
if (a == "Y" || a == "y") yaw_target_ = pos;
|
|
else if (a == "P" || a == "p") pitch_target_ = pos;
|
|
}
|
|
}
|
|
} else if (verb == "STOP") {
|
|
yaw_target_ = yaw_.xenc;
|
|
pitch_target_ = pitch_.xenc;
|
|
} else if (verb == "DUMP") {
|
|
// Mirror the firmware DUMP block so the `dump` path is demoable
|
|
// without hardware. Values are illustrative, not simulated.
|
|
std::ostringstream d;
|
|
d << "DUMP BEGIN build=mock uptime_ms=0 reset=POR\n"
|
|
<< "Y state=" << (homed_ ? "READY" : "RESET")
|
|
<< " xactual=" << yaw_.xenc << " xenc=" << yaw_.xenc
|
|
<< " DRV_STATUS=80084000 GSTAT=00\n"
|
|
<< "P state=" << (homed_ ? "READY" : "RESET")
|
|
<< " xactual=" << pitch_.xenc << " xenc=" << pitch_.xenc
|
|
<< " DRV_STATUS=80084000 GSTAT=00\n"
|
|
<< "DUMP END\n";
|
|
dump_ = d.str();
|
|
LOG_INFO << "firmware dump:\n" << dump_;
|
|
}
|
|
// ENABLE/DISABLE/SPEED/SETPOS/RESET: accepted, no simulation effect.
|
|
}
|
|
|
|
MotorTelemetry telemetry() override {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
step(yaw_, yaw_target_);
|
|
step(pitch_, pitch_target_);
|
|
MotorTelemetry t;
|
|
t.yaw = yaw_;
|
|
t.pitch = pitch_;
|
|
t.pitch_present = true;
|
|
return t;
|
|
}
|
|
|
|
std::string lastDump() override {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return dump_;
|
|
}
|
|
|
|
bool connected() const override { return true; }
|
|
|
|
private:
|
|
static constexpr long kStep = 80000; // counts advanced per telemetry read
|
|
|
|
void step(AxisTelemetry& a, long target) {
|
|
long d = target - a.xenc;
|
|
if (d >= -kStep && d <= kStep) {
|
|
a.xenc = target;
|
|
a.standstill = true;
|
|
} else {
|
|
a.xenc += (d > 0 ? kStep : -kStep);
|
|
a.standstill = false;
|
|
}
|
|
a.xactual = a.xenc;
|
|
a.state = homed_ ? AxisState::Ready : AxisState::Reset;
|
|
}
|
|
|
|
static void tryParse(const std::string& s, long& out) {
|
|
try {
|
|
out = std::stol(s);
|
|
} catch (const std::exception&) {}
|
|
}
|
|
|
|
std::mutex mutex_;
|
|
AxisTelemetry yaw_;
|
|
AxisTelemetry pitch_;
|
|
long yaw_target_ = 0;
|
|
long pitch_target_ = 0;
|
|
bool homed_ = false;
|
|
std::string dump_;
|
|
};
|
|
|
|
} // namespace fgc
|