Files
onstep/src/lib/axis/motor/servo/ServoDriver.cpp
T

171 lines
5.6 KiB
C++
Raw Normal View History

2026-05-20 21:48:13 +02:00
// -----------------------------------------------------------------------------------
// axis servo motor driver
#include "ServoDriver.h"
#ifdef SERVO_MOTOR_PRESENT
// the various servo driver models
#define SERVO_DRIVER_MODEL_COUNT (SERVO_DRIVER_LAST - SERVO_DRIVER_FIRST + 1)
#include "../../../gpioEx/GpioEx.h"
#include "../Motor.h"
#if DEBUG != OFF
const char* SERVO_DRIVER_NAME[SERVO_DRIVER_MODEL_COUNT] =
{
"SERVO_PE", // Phase (Direction)/Enable
"SERVO_EE", // Enable/Enable
"SERVO_TMC2130_DC", // TMC2130 w/ DC motor
"SERVO_TMC5160_DC", // TMC5160 w/ DC motor
"SERVO_TMC2209", // TMC2209 w/ stepper motor
"SERVO_TMC5160", // TMC5160 w/ stepper motor
"SERVO_KTECH" // KTech servo motor using velocity control
};
#endif
ServoDriver::ServoDriver(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings) {
if (axisNumber < 1 || axisNumber > 9) return;
this->axisNumber = axisNumber;
this->Pins = Pins;
if (Pins != NULL) {
enablePin = Pins->enable;
enabledState = Pins->enabledState;
faultPin = Pins->fault;
}
this->Settings = Settings;
driverModel = Settings->model;
statusMode = Settings->status;
acceleration.valueDefault = Settings->acceleration;
zeroDeadband.valueDefault = 0.0F;
}
bool ServoDriver::init(bool reverse) {
#if DEBUG == VERBOSE
VF("MSG:"); V(axisPrefix); VF("init model "); VL(SERVO_DRIVER_NAME[driverModel - SERVO_DRIVER_FIRST]);
VF("MSG:"); V(axisPrefix); VF("en=");
if (enablePin == OFF) { VLF("OFF"); } else
if (enablePin == SHARED) { VLF("SHARED"); } else { VL(enablePin); }
#endif
// init default driver control pins
if (enablePin != SHARED) {
pinModeEx(enablePin, OUTPUT);
digitalWriteEx(enablePin, !enabledState);
}
// automatically set fault status for known drivers
status.active = statusMode != OFF;
if (statusMode == ON) statusMode = LOW;
if (statusMode == LOW) pinModeEx(faultPin, INPUT_PULLUP);
#ifdef PULLDOWN
if (statusMode == HIGH) pinModeEx(faultPin, INPUT_PULLDOWN);
#else
if (statusMode == HIGH) pinModeEx(faultPin, INPUT);
#endif
reversed = reverse;
return true;
}
void ServoDriver::enable(bool state) {
enabled = state;
// Only touch a real, dedicated enable pin
if (enablePin != OFF && enablePin != SHARED) {
digitalWriteEx(enablePin, state ? enabledState : !enabledState);
}
// Default behavior: if disabled, force ramp to 0 so outputs converge quickly
if (!state) velocityRamp = 0.0F;
}
void ServoDriver::setFrequencyMax(float frequency) {
velocityMax = frequency;
InvVelocityMax = 1.0F/frequency;
normalizedAcceleration = (acceleration.value/100.0F)*velocityMax;
accelerationFs = normalizedAcceleration/FRACTIONAL_SEC;
// show velocity control settings
VF("MSG:"); V(axisPrefix); VF("Vmax="); V(velocityMax); VLF(" steps/s");
VF("MSG:"); V(axisPrefix); VF("Acceleration="); V(acceleration.value); VF("%/s/s ("); V(accelerationFs); VLF(" steps/s/fs)");
}
float ServoDriver::setMotorVelocity(float velocity) {
if (!enabled) velocity = 0.0F;
if (zeroDeadband.value > 0.0F && fabsf(velocity) < zeroDeadband.value) velocity = 0.0F;
if (velocity > velocityMax) velocity = velocityMax; else
if (velocity < -velocityMax) velocity = -velocityMax;
if(bypassAccelOnTracking)
velocityRamp = velocity;
else {
// ramp velocity
if (velocity > velocityRamp) {
velocityRamp += accelerationFs;
if (velocityRamp > velocity) velocityRamp = velocity;
} else
if (velocity < velocityRamp) {
velocityRamp -= accelerationFs;
if (velocityRamp < velocity) velocityRamp = velocity;
}
}
if (velocityRamp >= 0.0F) motorDirection = DIR_FORWARD; else motorDirection = DIR_REVERSE;
return velocityRamp;
}
// update status info. for driver
void ServoDriver::updateStatus() {
if (statusMode == ON) {
const unsigned long now = millis();
if ((long)(now - timeLastStatusUpdate) > 200L) {
readStatus();
// open load indication is not reliable in standstill
if (status.outputA.shortToGround ||
status.outputB.shortToGround ||
status.overTemperatureWarning ||
status.overTemperature) status.fault = true; else status.fault = false;
timeLastStatusUpdate = now;
}
}
#if DEBUG == VERBOSE
if (status.standstill) { status.outputA.openLoad = false; status.outputB.openLoad = false; status.standstill = false; }
if ((status.outputA.shortToGround != lastStatus.outputA.shortToGround) ||
(status.outputA.openLoad != lastStatus.outputA.openLoad) ||
(status.outputB.shortToGround != lastStatus.outputB.shortToGround) ||
(status.outputB.openLoad != lastStatus.outputB.openLoad) ||
(status.overTemperatureWarning != lastStatus.overTemperatureWarning) ||
(status.overTemperature != lastStatus.overTemperature) ||
(status.standstill != lastStatus.standstill) ||
(status.fault != lastStatus.fault)) {
VF("MSG:"); V(axisPrefix); VF("status change ");
VF("SGA"); if (status.outputA.shortToGround) VF("< "); else VF(". ");
VF("OLA"); if (status.outputA.openLoad) VF("< "); else VF(". ");
VF("SGB"); if (status.outputB.shortToGround) VF("< "); else VF(". ");
VF("OLB"); if (status.outputB.openLoad) VF("< "); else VF(". ");
VF("OTP"); if (status.overTemperatureWarning) VF("< "); else VF(". ");
VF("OTE"); if (status.overTemperature) VF("< "); else VF(". ");
VF("SST"); if (status.standstill) VF("< "); else VF(". ");
VF("FLT"); if (status.fault) VLF("<"); else VLF(".");
}
lastStatus = status;
#endif
};
#endif