185 lines
5.3 KiB
C++
185 lines
5.3 KiB
C++
// -----------------------------------------------------------------------------------
|
|||
|
|
// 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
|