Initial commit

TMC2209/lib/tmc/ramp/LinearRamp.c

@@ -0,0 +1,165 @@
+/*******************************************************************************
+* Copyright © 2018 TRINAMIC Motion Control GmbH & Co. KG
+* (now owned by Analog Devices Inc.),
+*
+* Copyright © 2023 Analog Devices Inc. All Rights Reserved. This software is
+* proprietary & confidential to Analog Devices, Inc. and its licensors.
+*******************************************************************************/
+
+/*
+ *  This is a basic proof-of-concept implementation of a linear motion ramp
+ *  generator. It is designed to run with 1 calculation / ms.
+ *
+ *
+ */
+#include "LinearRamp.h"
+
+void tmc_linearRamp_init(TMC_LinearRamp *linearRamp)
+{
+	linearRamp->maxVelocity     = 0;
+	linearRamp->targetPosition  = 0;
+	linearRamp->targetVelocity  = 0;
+	linearRamp->rampVelocity    = 0;
+	linearRamp->acceleration    = 0;
+	linearRamp->encoderSteps	= u16_MAX;
+	linearRamp->lastdVRest      = 0;
+	linearRamp->lastdXRest      = 0;
+	linearRamp->rampEnabled     = false;
+}
+
+void tmc_linearRamp_computeRampVelocity(TMC_LinearRamp *linearRamp)
+{
+	if (linearRamp->rampEnabled)
+	{
+		// update target velocity according actual set acceleration
+		// (scaling pre-factor of 1000 used for 1ms velocity ramp handling)
+
+		int32_t dV = linearRamp->acceleration;
+
+		// to ensure that small velocity changes at high set acceleration are also possible
+		int32_t maxDTV = abs(linearRamp->targetVelocity - linearRamp->rampVelocity);
+		if (maxDTV < (dV/1000))
+			dV = maxDTV*1000;
+
+		dV += linearRamp->lastdVRest;
+		linearRamp->lastdVRest = dV % 1000;
+
+		if (linearRamp->rampVelocity < linearRamp->targetVelocity)
+		{
+			// accelerate motor
+			linearRamp->rampVelocity += dV/1000;	// divide with pre-factor
+		}
+		else if (linearRamp->rampVelocity > linearRamp->targetVelocity)
+		{
+			// decelerate motor
+			linearRamp->rampVelocity -= dV/1000;	// divide with pre-factor
+		}
+	}
+	else
+	{
+		// use target velocity directly
+		linearRamp->rampVelocity = linearRamp->targetVelocity;
+	}
+
+	// limit ramp velocity
+	linearRamp->rampVelocity = tmc_limitInt(linearRamp->rampVelocity, -linearRamp->maxVelocity, linearRamp->maxVelocity);
+}
+
+void tmc_linearRamp_computeRampPosition(TMC_LinearRamp *linearRamp)
+{
+	if (linearRamp->rampEnabled)
+	{
+		// update target position according actual set acceleration and max velocity
+		// (scaling pre-factor of 1000 used for 1ms position ramp handling)
+
+		// limit position difference for further computations
+		int32_t targetPositionsDifference = linearRamp->targetPosition-linearRamp->rampPosition;
+
+		// limit the sqrti value in case of high position differences
+		int64_t sqrtiValue = tmc_limitS64(((int64_t)120 * (int64_t)linearRamp->acceleration * (int64_t)(abs(targetPositionsDifference))) / (int64_t)linearRamp->encoderSteps, 0, (int64_t)linearRamp->maxVelocity*(int64_t)linearRamp->maxVelocity);
+
+		// compute max allowed ramp velocity to ramp down to target
+		int32_t maxRampStop = tmc_sqrti(sqrtiValue);
+
+		// compute max allowed ramp velocity
+		int32_t maxRampTargetVelocity = 0;
+		if (targetPositionsDifference > 0)
+		{
+			maxRampTargetVelocity = tmc_limitInt(maxRampStop, 0, (int32_t)linearRamp->maxVelocity);
+		}
+		else if (targetPositionsDifference < 0)
+		{
+			maxRampTargetVelocity = tmc_limitInt(-maxRampStop, -(int32_t)linearRamp->maxVelocity, 0);
+		}
+		else
+		{
+			//maxRampTargetVelocity = 0;
+		}
+
+		int32_t dV = linearRamp->acceleration;  // pre-factor ~ 1/1000
+
+		// to ensure that small velocity changes at high set acceleration are also possible
+		int32_t maxDTV = abs(maxRampTargetVelocity - linearRamp->rampVelocity);
+		if (maxDTV < (dV / 1000))
+			dV = maxDTV * 1000;
+
+		dV += linearRamp->lastdVRest;
+		linearRamp->lastdVRest = dV % 1000;
+
+		// do velocity ramping
+		if (maxRampTargetVelocity > linearRamp->rampVelocity)
+		{
+			linearRamp->rampVelocity += dV / 1000;
+		}
+		else if (maxRampTargetVelocity < linearRamp->rampVelocity)
+		{
+			linearRamp->rampVelocity -= dV / 1000;
+		}
+
+		// limit positionRampTargetVelocity to maxRampTargetVelocity
+		//linearRamp->rampVelocity = tmc_limitInt(linearRamp->rampVelocity, -abs(maxRampTargetVelocity), abs(maxRampTargetVelocity));
+
+		// do position ramping using actual ramp velocity to update dX
+		int64_t dX = ((int64_t)linearRamp->rampVelocity * (int64_t)linearRamp->encoderSteps) / ((int64_t)60) + linearRamp->lastdXRest;
+
+		// scale actual target position
+		int64_t tempActualTargetPosition = (int64_t)linearRamp->rampPosition * 1000;
+
+		// reset helper variables if ramp position reached target position
+		if (abs(linearRamp->targetPosition - linearRamp->rampPosition) <= 10) /*abs(dX)*/
+		{
+			// sync ramp position with target position on small deviations
+			linearRamp->rampPosition = linearRamp->targetPosition;
+
+			// update actual target position
+			tempActualTargetPosition = (int64_t)linearRamp->rampPosition * 1000;
+
+			dX = 0;
+			linearRamp->lastdXRest = 0;
+			linearRamp->rampVelocity = 0;
+		}
+		else
+		{
+			// update actual target position
+			tempActualTargetPosition += dX;
+		}
+
+		int64_t absTempActualTargetPosition = (tempActualTargetPosition >= 0) ? tempActualTargetPosition : -tempActualTargetPosition;
+
+		if (tempActualTargetPosition >= 0)
+			linearRamp->lastdXRest = (absTempActualTargetPosition % 1000);
+		else if (tempActualTargetPosition < 0)
+			linearRamp->lastdXRest = -(absTempActualTargetPosition % 1000);
+
+		// scale actual target position back
+		linearRamp->rampPosition = tempActualTargetPosition / 1000;
+	}
+	else
+	{
+		// use target position directly
+		linearRamp->rampPosition = linearRamp->targetPosition;
+
+		// hold ramp velocity in reset
+		linearRamp->rampVelocity = 0;
+	}
+}