74 lines
2.4 KiB
C++
74 lines
2.4 KiB
C++
#include "fgc/AxisSpeed.h"
|
|
|
|
#include <cmath>
|
|
|
|
namespace fgc {
|
|
namespace {
|
|
|
|
std::string num(double v) {
|
|
char buf[32];
|
|
std::snprintf(buf, sizeof(buf), "%.3g", v);
|
|
return buf;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
double vmaxToDegPerSec(long vmax, double counts_per_deg, double clock_hz) {
|
|
const double cpd = std::fabs(counts_per_deg);
|
|
if (cpd <= 0.0 || clock_hz <= 0.0) return 0.0;
|
|
return (static_cast<double>(vmax) * clock_hz / kTmcVmaxDiv) / cpd;
|
|
}
|
|
|
|
AxisSpeedPlan planAxisSpeed(double deg_per_s, double counts_per_deg, double clock_hz) {
|
|
AxisSpeedPlan p;
|
|
p.requested_deg_s = deg_per_s;
|
|
|
|
// A speed is a magnitude; the direction sense lives in counts_per_deg's sign
|
|
// and belongs to Geometry, not here.
|
|
const double cpd = std::fabs(counts_per_deg);
|
|
|
|
if (deg_per_s <= 0.0) {
|
|
p.error = "speed must be > 0 deg/s, got " + num(deg_per_s);
|
|
return p;
|
|
}
|
|
if (cpd < kMinPlausibleCountsPerDeg) {
|
|
p.error = "counts_per_deg is " + num(counts_per_deg) +
|
|
", which is not a calibrated drivetrain (an ungeared AS5047P alone reads 11.1) "
|
|
"-- calibrate the axis first ('gimbal heading setup'), or declare "
|
|
"[Motor.<axis>] travel_deg so homing can derive it";
|
|
return p;
|
|
}
|
|
if (clock_hz <= 0.0) {
|
|
p.error = "TMC clock must be > 0 Hz";
|
|
return p;
|
|
}
|
|
|
|
const double counts_per_s = deg_per_s * cpd;
|
|
const double exact = counts_per_s * kTmcVmaxDiv / clock_hz;
|
|
|
|
// Round rather than truncate: at low speeds truncation biases every axis slow,
|
|
// and at 1 count/s it would round to zero and stop the gimbal outright.
|
|
long vmax = std::lround(exact);
|
|
|
|
if (vmax > kTmcVmaxMax) {
|
|
vmax = kTmcVmaxMax;
|
|
p.clamped = true;
|
|
}
|
|
if (vmax < 1) {
|
|
// Anything that rounds below one unit cannot be commanded at all. Failing
|
|
// is right: silently substituting the slowest possible speed would look
|
|
// like a working axis that never finishes a sweep.
|
|
p.error = num(deg_per_s) + " deg/s is below what VMAX can express on this drivetrain (" +
|
|
num(cpd) + " counts/deg); the slowest commandable speed is " +
|
|
num(vmaxToDegPerSec(1, cpd, clock_hz)) + " deg/s";
|
|
return p;
|
|
}
|
|
|
|
p.ok = true;
|
|
p.vmax = vmax;
|
|
p.actual_deg_s = vmaxToDegPerSec(vmax, cpd, clock_hz);
|
|
return p;
|
|
}
|
|
|
|
} // namespace fgc
|