// ----------------------------------------------------------------------------------- // 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