This commit is contained in:
sagiferi
2026-05-20 21:48:13 +02:00
parent a907d8ba74
commit d3c13f997e
446 changed files with 70716 additions and 0 deletions
+454
View File
@@ -0,0 +1,454 @@
// -----------------------------------------------------------------------------------
// axis MKS SERVO42D/57D motor driver via CAN, using 0xFE absolute position updates
// EXPERIMENTAL!!!
#include "MksServo.h"
#ifdef MKS42D_MOTOR_PRESENT
#include "../../../tasks/OnTask.h"
#include "../../../canPlus/CanPlus.h"
static Mks42DMotor *mksMotorInstance[9] = {nullptr};
IRAM_ATTR void moveMksAxis1() { mksMotorInstance[0]->move(); }
IRAM_ATTR void moveMksAxis2() { mksMotorInstance[1]->move(); }
IRAM_ATTR void moveMksAxis3() { mksMotorInstance[2]->move(); }
IRAM_ATTR void moveMksAxis4() { mksMotorInstance[3]->move(); }
IRAM_ATTR void moveMksAxis5() { mksMotorInstance[4]->move(); }
IRAM_ATTR void moveMksAxis6() { mksMotorInstance[5]->move(); }
IRAM_ATTR void moveMksAxis7() { mksMotorInstance[6]->move(); }
IRAM_ATTR void moveMksAxis8() { mksMotorInstance[7]->move(); }
IRAM_ATTR void moveMksAxis9() { mksMotorInstance[8]->move(); }
// Status callbacks (0xF1 and 0x3E both funnel into requestStatusCallback)
static void statusMksAxis1(uint8_t data[8]) { if (mksMotorInstance[0]) mksMotorInstance[0]->requestStatusCallback(data); }
static void statusMksAxis2(uint8_t data[8]) { if (mksMotorInstance[1]) mksMotorInstance[1]->requestStatusCallback(data); }
static void statusMksAxis3(uint8_t data[8]) { if (mksMotorInstance[2]) mksMotorInstance[2]->requestStatusCallback(data); }
static void statusMksAxis4(uint8_t data[8]) { if (mksMotorInstance[3]) mksMotorInstance[3]->requestStatusCallback(data); }
static void statusMksAxis5(uint8_t data[8]) { if (mksMotorInstance[4]) mksMotorInstance[4]->requestStatusCallback(data); }
static void statusMksAxis6(uint8_t data[8]) { if (mksMotorInstance[5]) mksMotorInstance[5]->requestStatusCallback(data); }
static void statusMksAxis7(uint8_t data[8]) { if (mksMotorInstance[6]) mksMotorInstance[6]->requestStatusCallback(data); }
static void statusMksAxis8(uint8_t data[8]) { if (mksMotorInstance[7]) mksMotorInstance[7]->requestStatusCallback(data); }
static void statusMksAxis9(uint8_t data[8]) { if (mksMotorInstance[8]) mksMotorInstance[8]->requestStatusCallback(data); }
Mks42DMotor::Mks42DMotor(uint8_t axisNumber,
int8_t reverse,
const MksDriverSettings *Settings,
bool useFastHardwareTimers)
: Motor(axisNumber, reverse) {
if (axisNumber < 1 || axisNumber > 9) return;
driverType = ODRIVER;
strcpy(axisPrefix, " Axis_Mks42D, ");
axisPrefix[5] = '0' + axisNumber;
// Mapping policy: CAN ID == axis number (1..9)
canID = axisNumber;
// reserve settings for future protocol/status differences
if (Settings) {
model = Settings->model;
statusMode = Settings->status;
} else {
model = 0;
statusMode = OFF;
}
// AxisParameter defaults (must be assigned here)
// stepsPerSecondToRpm is intentionally *not* used in the simple/robust mode below.
stepsPerSecondToRpm.valueDefault = 0.01F;
maxRpm.valueDefault = 60.0F;
if (axisNumber > 2) useFastHardwareTimers = false;
this->useFastHardwareTimers = useFastHardwareTimers;
mksMotorInstance[axisNumber - 1] = this;
switch (axisNumber) {
case 1: callback = moveMksAxis1; break;
case 2: callback = moveMksAxis2; break;
case 3: callback = moveMksAxis3; break;
case 4: callback = moveMksAxis4; break;
case 5: callback = moveMksAxis5; break;
case 6: callback = moveMksAxis6; break;
case 7: callback = moveMksAxis7; break;
case 8: callback = moveMksAxis8; break;
case 9: callback = moveMksAxis9; break;
}
}
bool Mks42DMotor::init() {
if (!Motor::init()) return false;
if (!canPlus.ready) {
V("ERR:"); V(axisPrefix); VLF("No CAN interface!");
return false;
}
ready = true;
enable(false);
VF("MSG:"); V(axisPrefix); VF("start task to synthesize motion... ");
char timerName[] = "Ax_Mks_";
timerName[2] = axisNumber + '0';
taskHandle = tasks.add(0, 0, true, 0, callback, timerName);
if (taskHandle) {
VF("success");
if (useFastHardwareTimers) {
if (!tasks.requestHardwareTimer(taskHandle, 0)) {
VF(" (no hardware timer!)");
} else {
maxFrequency = (1000000.0F / HAL_MAXRATE_LOWER_LIMIT) / 2.0F;
}
}
VLF("");
} else {
VLF("FAILED!");
ready = false;
return false;
}
stopSyntheticMotion();
resetToTrackingBaseline();
// Always-on heartbeat + protect polling (RX callbacks)
if (statusMode == ON) {
switch (axisNumber) {
case 1:
canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis1);
canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis1);
break;
case 2:
canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis2);
canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis2);
break;
case 3:
canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis3);
canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis3);
break;
case 4:
canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis4);
canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis4);
break;
case 5:
canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis5);
canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis5);
break;
case 6:
canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis6);
canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis6);
break;
case 7:
canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis7);
canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis7);
break;
case 8:
canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis8);
canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis8);
break;
case 9:
canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis9);
canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis9);
break;
}
statusValid = false;
protectActive = false;
status.fault = false;
lastStatusUpdateTime = 0;
lastStatusRequestTime = 0;
lastProtectRequestTime = 0;
}
return true;
}
void Mks42DMotor::setReverse(int8_t state) {
if (!ready) return;
if (state == ON) {
V("WRN:"); V(axisPrefix); VLF("axis reversal must be done in MKS setup or wiring!");
}
}
void Mks42DMotor::setReverse(bool state) {
if (state) {
V("WRN:"); V(axisPrefix); VLF("software reverse requested; prefer MKS setup or wiring!");
}
Motor::setReverse(state);
}
void Mks42DMotor::enable(bool state) {
if (!ready || state == enabled) return;
if (!state) enabled = false;
stopSyntheticMotion();
resetToTrackingBaseline();
VF("MSG:"); V(axisPrefix); VF("driver powered "); VLF(state ? "up" : "down");
canPlus.txWait();
sendF3(state);
if (state) {
enabled = true;
lastEnableTime = millis();
// clear latched faults on enable attempt (simple policy for v1)
status.fault = false;
protectActive = false;
// statusValid remains as-is; always-on polling will refresh it soon
}
// force next poll() to send target
lastTarget = LONG_MIN;
}
void Mks42DMotor::resetPositionSteps(long value) {
if (!ready) return;
Motor::resetPositionSteps(value);
// force next poll() to send target
lastTarget = LONG_MIN;
}
float Mks42DMotor::getFrequencySteps() {
if (!ready) return 0.0F;
if (lastPeriod == 0) return 0.0F;
return (16000000.0F / lastPeriod) * absStep;
}
void Mks42DMotor::setFrequencySteps(float frequency) {
if (!ready) return;
if (!enabled) { stopSyntheticMotion(); return; }
int dir = 0;
if (frequency > 0.0F) dir = 1;
else if (frequency < 0.0F) { frequency = -frequency; dir = -1; }
if (inBacklash) frequency = backlashFrequency;
if (frequency != currentFrequency) {
// compensate for performance limitations by taking larger steps as needed
if (frequency < maxFrequency) stepSize = 1; else
if (frequency < maxFrequency*2) stepSize = 2; else
if (frequency < maxFrequency*4) stepSize = 4; else
if (frequency < maxFrequency*8) stepSize = 8; else
if (frequency < maxFrequency*16) stepSize = 16; else
if (frequency < maxFrequency*32) stepSize = 32; else
if (frequency < maxFrequency*64) stepSize = 64; else
if (frequency < maxFrequency*128) stepSize = 128; else stepSize = 256;
// timer period in microseconds
float period = (1000000.0F * stepSize) / frequency;
// range is 0 to 134 seconds/step
if (!isnan(period) && period <= 130000000.0F) {
period *= 16.0F;
lastPeriod = (unsigned long)lroundf(period);
} else {
lastPeriod = 0;
frequency = 0.0F;
dir = 0;
}
currentFrequency = frequency;
// change the motor rate/direction
noInterrupts();
step = 0;
interrupts();
tasks.setPeriodSubMicros(taskHandle, lastPeriod);
}
noInterrupts();
step = dir * stepSize;
absStep = (int)labs(step);
interrupts();
}
void Mks42DMotor::setSlewing(bool state) {
if (!ready) return;
isSlewing = state;
}
// ---- CAN helpers ----
inline void Mks42DMotor::sendWithCrc(const uint8_t *payload, size_t n) {
if (!payload || n < 1 || n > 7) return;
uint8_t buf[8] = {0};
memcpy(buf, payload, n);
buf[n] = crc8((uint16_t)canID, buf, n);
canPlus.writePacket(canID, buf, n + 1);
}
inline void Mks42DMotor::sendF3(bool en) {
const uint8_t p[2] = { 0xF3, (uint8_t)(en ? 1 : 0) };
sendWithCrc(p, 2);
}
inline void Mks42DMotor::sendFE(int32_t pos, uint16_t speed) {
const int32_t p = clamp24(pos);
const uint32_t u = (uint32_t)p & 0x00FFFFFFUL; // signed 24-bit in low 24 bits
const uint8_t payload[7] = {
0xFE,
(uint8_t)(speed >> 8),
(uint8_t)(speed & 0xFF),
(uint8_t)MKS_FE_ACCEL_CONST,
(uint8_t)((u >> 16) & 0xFF),
(uint8_t)((u >> 8) & 0xFF),
(uint8_t)((u >> 0) & 0xFF),
};
sendWithCrc(payload, 7);
}
// Always-on polling commands
inline void Mks42DMotor::sendF1() { const uint8_t p[1] = { 0xF1 }; sendWithCrc(p, 1); }
inline void Mks42DMotor::send3E() { const uint8_t p[1] = { 0x3E }; sendWithCrc(p, 1); }
// RX: treat 0xF1 and 0x3E as heartbeat sources; latch fault on protect active.
void Mks42DMotor::requestStatusCallback(uint8_t data[8]) {
if (statusMode == OFF) return;
const uint8_t code = data[0];
if (code != 0xF1 && code != 0x3E) return;
lastStatusUpdateTime = millis();
statusValid = true;
if (code == 0x3E) {
protectActive = (data[1] != 0);
if (protectActive) status.fault = true; // latch
}
// Optional extension hook: decode 0xF1 state from data[1] later if desired.
}
bool Mks42DMotor::hasHeartbeat(uint32_t maxAgeMs) const {
if (!ready || statusMode == OFF) return false;
if (!statusValid) return false;
const unsigned long now = millis();
return (now - lastStatusUpdateTime) < maxAgeMs;
}
// Absolute position streaming (simple/robust):
// - Stream 0xFE target position
// - Use a fixed speed cap (maxRpm) rather than a dynamic feed-forward mapping
void Mks42DMotor::poll() {
if (!ready) return;
const unsigned long now = millis();
// Always-on heartbeat + protect polling
if (statusMode == ON) {
if ((now - lastStatusRequestTime) >= MKS_STATUS_MS) {
lastStatusRequestTime = now;
if (canPlus.txTryLock()) sendF1(); // best-effort, non-blocking
}
if ((now - lastProtectRequestTime) >= MKS_PROTECT_MS) {
lastProtectRequestTime = now;
if (canPlus.txTryLock()) send3E(); // best-effort, non-blocking
}
// Hard faults only when enabled, with grace window after enabling
if (enabled) {
const bool inGrace = (now - lastEnableTime) < (unsigned long)MKS_HEARTBEAT_GRACE_MS;
const bool ok = statusValid ? hasHeartbeat(MKS_STATUS_MS * 4U) : inGrace;
if (!ok || status.fault) {
enabled = false;
stopSyntheticMotion();
resetToTrackingBaseline();
return;
}
}
}
if (!enabled) return;
noInterrupts();
#if MKS_SLEW_DIRECT == ON
long target = targetSteps + backlashSteps;
#else
long target = motorSteps + backlashSteps;
#endif
interrupts();
// signed 24-bit domain enforced here
const int32_t pos = clamp24((int32_t)target);
// simple: speed is a cap (RPM), not a feed-forward term
uint16_t spd = (uint16_t)lroundf(pval(maxRpm));
if (spd > 3000) spd = 3000;
// send only on position change (avoid any "speed jitter" surprises)
if (lastTarget == (long)pos) return;
if (!canPlus.txTryLock()) return;
sendFE(pos, spd);
lastTarget = (long)pos;
}
void Mks42DMotor::stopSyntheticMotion() {
if (lastPeriod == 0 && step == 0 && absStep == 0) return;
currentFrequency = 0.0F;
lastPeriod = 0;
noInterrupts();
step = 0;
absStep = 0;
interrupts();
if (taskHandle) tasks.setPeriodSubMicros(taskHandle, 0);
}
void Mks42DMotor::resetToTrackingBaseline() {
noInterrupts();
step = 0;
absStep = 0;
inBacklash = false;
backlashSteps = 0;
interrupts();
currentFrequency = 0.0F;
lastPeriod = 0;
stepSize = 1;
// force next poll() to send target
lastTarget = LONG_MIN;
}
// sets dir as required and moves coord toward target at setFrequencySteps() rate
IRAM_ATTR void Mks42DMotor::move() {
if (sync && !inBacklash) targetSteps += step;
if (motorSteps > targetSteps) {
if (backlashSteps > 0) {
backlashSteps -= absStep;
inBacklash = backlashSteps > 0;
} else {
motorSteps -= absStep;
inBacklash = false;
}
} else if (motorSteps < targetSteps || inBacklash) {
if (backlashSteps < backlashAmountSteps) {
backlashSteps += absStep;
inBacklash = backlashSteps < backlashAmountSteps;
} else {
motorSteps += absStep;
inBacklash = false;
}
}
}
#endif
+184
View File
@@ -0,0 +1,184 @@
// -----------------------------------------------------------------------------------
// axis MKS SERVO42D/57D motor driver via CAN, using 0xFE absolute position updates
// EXPERIMENTAL!!!
#pragma once
#include "../../../../Common.h"
#ifdef MKS42D_MOTOR_PRESENT
#if !defined(CAN_PLUS) || CAN_PLUS == OFF
#error "No MKS motor CAN interface!"
#endif
#include "../Motor.h"
#ifndef MKS_SLEW_DIRECT
#define MKS_SLEW_DIRECT OFF
#endif
#ifndef MKS_FE_ACCEL_CONST
#define MKS_FE_ACCEL_CONST 10
#endif
// always-on status request (0xF1) rate
#ifndef MKS_STATUS_MS
#define MKS_STATUS_MS 250
#endif
// always-on protect-state request (0x3E) rate
#ifndef MKS_PROTECT_MS
#define MKS_PROTECT_MS 1000
#endif
// grace period after enabling before enforcing heartbeat
#ifndef MKS_HEARTBEAT_GRACE_MS
#define MKS_HEARTBEAT_GRACE_MS 1500
#endif
typedef struct MksDriverSettings {
int16_t model;
int8_t status; // ON/OFF (enables always-on heartbeat + protect polling)
} MksDriverSettings;
class Mks42DMotor : public Motor {
public:
Mks42DMotor(uint8_t axisNumber,
int8_t reverse,
const MksDriverSettings *Settings,
bool useFastHardwareTimers = true);
bool init() override;
uint8_t getParameterCount() override { return Motor::getParameterCount() + numParameters; }
AxisParameter* getParameter(uint8_t number) override {
const uint8_t base = Motor::getParameterCount();
if (number >= 1 && number <= base) return Motor::getParameter(number);
const uint8_t i = number - base;
if (i == 1) return &stepsPerSecondToRpm;
if (i == 2) return &maxRpm;
return &invalid;
}
// Integrated-driver style reverse control (kept for parity with other integrated CAN motors)
void setReverse(int8_t state);
// Base-class reverse (prevents silent bypass if someone calls Motor::setReverse(bool))
void setReverse(bool state) override;
void enable(bool state) override;
void resetPositionSteps(long value) override;
int getStepsPerStepSlewing() override { return 256; }
float getFrequencySteps() override;
void setFrequencySteps(float frequency) override;
void setSlewing(bool state) override;
void poll() override;
const char* name() override { return "MKS_FE"; }
// ISR uses this; not a Motor virtual
void move();
// minimal heartbeat API (updated by 0xF1 or 0x3E callbacks)
bool hasHeartbeat(uint32_t maxAgeMs) const;
// driver status (minimal; only "fault" is currently meaningful)
DriverStatus getDriverStatus() { return ready ? status : errorStatus; }
// RX callback (wired via CanPlus callbackRegisterMessage)
void requestStatusCallback(uint8_t data[8]);
private:
static inline uint8_t crc8(uint16_t id11, const uint8_t *data, size_t n) {
uint32_t sum = (uint32_t)(id11 & 0x7FF);
for (size_t i = 0; i < n; i++) sum += data[i];
return (uint8_t)(sum & 0xFF);
}
static inline int32_t clamp24(int32_t v) {
if (v > 8388607L) return 8388607L;
if (v < -8388608L) return -8388608L;
return v;
}
inline float pval(const AxisParameter &p) const {
const float v = p.value;
return isnan(v) ? p.valueDefault : v;
}
// stepsPerSecondToRpm unit: RPM per (steps/second)
inline uint16_t speedFromStepRate(float stepsPerSec) const {
if (stepsPerSec < 0.0F) stepsPerSec = -stepsPerSec;
const float scale = pval(stepsPerSecondToRpm);
float sF = stepsPerSec * scale;
if (sF < 0.0F) sF = 0.0F;
// MKS protocol speed range is 0..3000 RPM; keep the clamp aligned to that.
float capF = pval(maxRpm);
if (capF < 0.0F) capF = 0.0F;
if (capF > 3000.0F) capF = 3000.0F;
if (sF > capF) sF = capF;
return (uint16_t)lroundf(sF);
}
inline void sendWithCrc(const uint8_t *payload, size_t n); // n<=7
inline void sendF3(bool en);
inline void sendFE(int32_t pos, uint16_t speed);
// Always-on polling helpers
inline void sendF1(); // queryStatus (0xF1)
inline void send3E(); // readProtectState (0x3E)
void stopSyntheticMotion();
void resetToTrackingBaseline();
int canID = 0;
// reserved / future
int16_t model = 0;
int8_t statusMode = OFF;
uint8_t taskHandle = 0;
int stepSize = 1;
float currentFrequency = 0.0F;
unsigned long lastPeriod = 0;
float maxFrequency = HAL_FRACTIONAL_SEC;
volatile int absStep = 0;
void (*callback)() = nullptr;
bool useFastHardwareTimers = true;
bool isSlewing = false;
long lastTarget = LONG_MIN;
// --- always-on heartbeat / protect state ---
unsigned long lastStatusRequestTime = 0;
unsigned long lastProtectRequestTime = 0;
unsigned long lastStatusUpdateTime = 0;
unsigned long lastEnableTime = 0;
bool statusValid = false;
bool protectActive = false; // latched via 0x3E
// Minimal status model: only "fault" is currently meaningful
DriverStatus status = { false, {false, false}, {false, false}, false, false, false, false };
// runtime adjustable settings
AxisParameter stepsPerSecondToRpm = {NAN, NAN, NAN, 0.0F, 10000.0F, AXP_FLOAT, "Steps/s to RPM"};
AxisParameter maxRpm = {NAN, NAN, NAN, 0.0F, 3000.0F, AXP_INTEGER, "Max Speed RPM"};
const int numParameters = 2;
};
#endif