Initial commit
This commit is contained in:
165
TMC2209/lib/tmc/ramp/LinearRamp.c
Normal file
165
TMC2209/lib/tmc/ramp/LinearRamp.c
Normal file
@@ -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;
|
||||
}
|
||||
}
|
||||
34
TMC2209/lib/tmc/ramp/LinearRamp.h
Normal file
34
TMC2209/lib/tmc/ramp/LinearRamp.h
Normal file
@@ -0,0 +1,34 @@
|
||||
/*******************************************************************************
|
||||
* 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.
|
||||
*******************************************************************************/
|
||||
|
||||
|
||||
#ifndef TMC_LINEAR_RAMP_H_
|
||||
#define TMC_LINEAR_RAMP_H_
|
||||
|
||||
#include "tmc/helpers/API_Header.h"
|
||||
#include "tmc/helpers/Functions.h"
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint32_t maxVelocity;
|
||||
int32_t targetPosition;
|
||||
int32_t rampPosition;
|
||||
int32_t targetVelocity;
|
||||
int32_t rampVelocity;
|
||||
int32_t acceleration;
|
||||
uint16_t encoderSteps;
|
||||
int32_t lastdVRest;
|
||||
int32_t lastdXRest;
|
||||
uint8_t rampEnabled;
|
||||
} TMC_LinearRamp;
|
||||
|
||||
void tmc_linearRamp_init(TMC_LinearRamp *linearRamp);
|
||||
void tmc_linearRamp_computeRampVelocity(TMC_LinearRamp *linearRamp);
|
||||
void tmc_linearRamp_computeRampPosition(TMC_LinearRamp *linearRamp);
|
||||
|
||||
#endif /* TMC_LINEAR_RAMP_H_ */
|
||||
303
TMC2209/lib/tmc/ramp/LinearRamp1.c
Normal file
303
TMC2209/lib/tmc/ramp/LinearRamp1.c
Normal file
@@ -0,0 +1,303 @@
|
||||
/*******************************************************************************
|
||||
* 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.
|
||||
*******************************************************************************/
|
||||
|
||||
|
||||
#include "LinearRamp1.h"
|
||||
#include "tmc/helpers/Functions.h"
|
||||
|
||||
void tmc_ramp_linear_init(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
linearRamp->maxVelocity = 0;
|
||||
linearRamp->targetPosition = 0;
|
||||
linearRamp->targetVelocity = 0;
|
||||
linearRamp->rampVelocity = 0;
|
||||
linearRamp->rampPosition = 0;
|
||||
linearRamp->acceleration = 0;
|
||||
linearRamp->rampEnabled = true;
|
||||
linearRamp->accumulatorVelocity = 0;
|
||||
linearRamp->accumulatorPosition = 0;
|
||||
linearRamp->rampMode = TMC_RAMP_LINEAR_MODE_VELOCITY;
|
||||
linearRamp->state = TMC_RAMP_LINEAR_STATE_IDLE;
|
||||
linearRamp->precision = TMC_RAMP_LINEAR_DEFAULT_PRECISION;
|
||||
linearRamp->homingDistance = TMC_RAMP_LINEAR_DEFAULT_HOMING_DISTANCE;
|
||||
linearRamp->stopVelocity = TMC_RAMP_LINEAR_DEFAULT_STOP_VELOCITY;
|
||||
}
|
||||
|
||||
void tmc_ramp_linear_set_enabled(TMC_LinearRamp *linearRamp, bool enabled)
|
||||
{
|
||||
linearRamp->rampEnabled = enabled;
|
||||
}
|
||||
|
||||
void tmc_ramp_linear_set_maxVelocity(TMC_LinearRamp *linearRamp, uint32_t maxVelocity)
|
||||
{
|
||||
linearRamp->maxVelocity = maxVelocity;
|
||||
}
|
||||
|
||||
void tmc_ramp_linear_set_targetPosition(TMC_LinearRamp *linearRamp, int32_t targetPosition)
|
||||
{
|
||||
linearRamp->targetPosition = targetPosition;
|
||||
}
|
||||
|
||||
void tmc_ramp_linear_set_rampPosition(TMC_LinearRamp *linearRamp, int32_t rampPosition)
|
||||
{
|
||||
linearRamp->rampPosition = rampPosition;
|
||||
}
|
||||
|
||||
void tmc_ramp_linear_set_targetVelocity(TMC_LinearRamp *linearRamp, int32_t targetVelocity)
|
||||
{
|
||||
linearRamp->targetVelocity = targetVelocity;
|
||||
}
|
||||
|
||||
void tmc_ramp_linear_set_rampVelocity(TMC_LinearRamp *linearRamp, int32_t rampVelocity)
|
||||
{
|
||||
linearRamp->rampVelocity = rampVelocity;
|
||||
}
|
||||
|
||||
void tmc_ramp_linear_set_acceleration(TMC_LinearRamp *linearRamp, int32_t acceleration)
|
||||
{
|
||||
linearRamp->acceleration = acceleration;
|
||||
}
|
||||
|
||||
void tmc_ramp_linear_set_mode(TMC_LinearRamp *linearRamp, TMC_LinearRamp_Mode mode)
|
||||
{
|
||||
linearRamp->rampMode = mode;
|
||||
}
|
||||
|
||||
void tmc_ramp_linear_set_precision(TMC_LinearRamp * linearRamp, uint32_t precision)
|
||||
{
|
||||
linearRamp->precision = precision;
|
||||
}
|
||||
|
||||
void tmc_ramp_linear_set_homingDistance(TMC_LinearRamp *linearRamp, uint32_t homingDistance)
|
||||
{
|
||||
linearRamp->homingDistance = homingDistance;
|
||||
}
|
||||
|
||||
void tmc_ramp_linear_set_stopVelocity(TMC_LinearRamp *linearRamp, uint32_t stopVelocity)
|
||||
{
|
||||
linearRamp->stopVelocity = stopVelocity;
|
||||
}
|
||||
|
||||
bool tmc_ramp_linear_get_enabled(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
return linearRamp->rampEnabled;
|
||||
}
|
||||
|
||||
uint32_t tmc_ramp_linear_get_maxVelocity(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
return linearRamp->maxVelocity;
|
||||
}
|
||||
|
||||
int32_t tmc_ramp_linear_get_targetPosition(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
return linearRamp->targetPosition;
|
||||
}
|
||||
|
||||
int32_t tmc_ramp_linear_get_rampPosition(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
return linearRamp->rampPosition;
|
||||
}
|
||||
|
||||
int32_t tmc_ramp_linear_get_targetVelocity(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
return linearRamp->targetVelocity;
|
||||
}
|
||||
|
||||
int32_t tmc_ramp_linear_get_rampVelocity(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
return linearRamp->rampVelocity;
|
||||
}
|
||||
|
||||
int32_t tmc_ramp_linear_get_acceleration(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
return linearRamp->acceleration;
|
||||
}
|
||||
|
||||
TMC_LinearRamp_State tmc_ramp_linear_get_state(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
return linearRamp->state;
|
||||
}
|
||||
|
||||
TMC_LinearRamp_Mode tmc_ramp_linear_get_mode(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
return linearRamp->rampMode;
|
||||
}
|
||||
|
||||
uint32_t tmc_ramp_linear_get_precision(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
return linearRamp->precision;
|
||||
}
|
||||
|
||||
// The maximum acceleration depends on the precision value
|
||||
uint32_t tmc_ramp_linear_get_acceleration_limit(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
return (0xFFFFFFFFu / linearRamp->precision) * linearRamp->precision;
|
||||
}
|
||||
|
||||
uint32_t tmc_ramp_linear_get_velocity_limit(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
return linearRamp->precision;
|
||||
}
|
||||
|
||||
uint32_t tmc_ramp_linear_get_homingDistance(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
return linearRamp->homingDistance;
|
||||
}
|
||||
|
||||
uint32_t tmc_ramp_linear_get_stopVelocity(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
return linearRamp->stopVelocity;
|
||||
}
|
||||
|
||||
int32_t tmc_ramp_linear_compute(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
tmc_ramp_linear_compute_position(linearRamp);
|
||||
return tmc_ramp_linear_compute_velocity(linearRamp);
|
||||
}
|
||||
|
||||
int32_t tmc_ramp_linear_compute_velocity(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
bool accelerating = linearRamp->rampVelocity != linearRamp->targetVelocity;
|
||||
|
||||
if (linearRamp->rampEnabled)
|
||||
{
|
||||
// Add current acceleration to accumulator
|
||||
linearRamp->accumulatorVelocity += linearRamp->acceleration;
|
||||
|
||||
// Calculate the velocity delta value and keep the remainder of the velocity accumulator
|
||||
int32_t dv = linearRamp->accumulatorVelocity / linearRamp->precision;
|
||||
linearRamp->accumulatorVelocity = linearRamp->accumulatorVelocity % linearRamp->precision;
|
||||
|
||||
// Add dv to rampVelocity, and regulate to target velocity
|
||||
if(linearRamp->rampVelocity < linearRamp->targetVelocity)
|
||||
linearRamp->rampVelocity = MIN(linearRamp->rampVelocity + dv, linearRamp->targetVelocity);
|
||||
else if(linearRamp->rampVelocity > linearRamp->targetVelocity)
|
||||
linearRamp->rampVelocity = MAX(linearRamp->rampVelocity - dv, linearRamp->targetVelocity);
|
||||
}
|
||||
else
|
||||
{
|
||||
// use target velocity directly
|
||||
linearRamp->rampVelocity = linearRamp->targetVelocity;
|
||||
// Reset accumulator
|
||||
linearRamp->accumulatorVelocity = 0;
|
||||
}
|
||||
|
||||
// Calculate the velocity delta value and keep the remainder of the position accumulator
|
||||
linearRamp->accumulatorPosition += linearRamp->rampVelocity;
|
||||
int32_t dx = linearRamp->accumulatorPosition / (int32_t) linearRamp->precision;
|
||||
linearRamp->accumulatorPosition = linearRamp->accumulatorPosition % (int32_t) linearRamp->precision;
|
||||
|
||||
if(dx == 0)
|
||||
return dx;
|
||||
|
||||
// Change actual position determined by position change
|
||||
linearRamp->rampPosition += (dx < 0) ? (-1) : (1);
|
||||
|
||||
// Count acceleration steps needed for decelerating later
|
||||
linearRamp->accelerationSteps += (abs(linearRamp->rampVelocity) < abs(linearRamp->targetVelocity)) ? accelerating : -accelerating;
|
||||
if (linearRamp->accelerationSteps < 0)
|
||||
linearRamp->accelerationSteps = 0;
|
||||
|
||||
return dx;
|
||||
}
|
||||
|
||||
void tmc_ramp_linear_compute_position(TMC_LinearRamp *linearRamp)
|
||||
{
|
||||
if (!linearRamp->rampEnabled)
|
||||
return;
|
||||
|
||||
if (linearRamp->rampMode != TMC_RAMP_LINEAR_MODE_POSITION)
|
||||
return;
|
||||
|
||||
// Calculate steps needed to target
|
||||
int32_t diffx = 0;
|
||||
|
||||
switch(linearRamp->state) {
|
||||
case TMC_RAMP_LINEAR_STATE_IDLE:
|
||||
if(linearRamp->rampVelocity == 0)
|
||||
linearRamp->accelerationSteps = 0;
|
||||
|
||||
if(linearRamp->rampPosition == linearRamp->targetPosition)
|
||||
break;
|
||||
|
||||
linearRamp->state = TMC_RAMP_LINEAR_STATE_DRIVING;
|
||||
break;
|
||||
case TMC_RAMP_LINEAR_STATE_DRIVING:
|
||||
// Calculate distance to target (positive = driving towards target)
|
||||
if(linearRamp->rampVelocity > 0)
|
||||
diffx = linearRamp->targetPosition - linearRamp->rampPosition;
|
||||
else if(linearRamp->rampVelocity < 0)
|
||||
diffx = -(linearRamp->targetPosition - linearRamp->rampPosition);
|
||||
else
|
||||
diffx = abs(linearRamp->targetPosition - linearRamp->rampPosition);
|
||||
|
||||
// Steps left required for braking?
|
||||
// (+ 1 to compensate rounding (flooring) errors of the position accumulator)
|
||||
if(linearRamp->accelerationSteps + 1 >= diffx)
|
||||
{
|
||||
linearRamp->targetVelocity = 0;
|
||||
linearRamp->state = TMC_RAMP_LINEAR_STATE_BRAKING;
|
||||
}
|
||||
else
|
||||
{ // Driving - apply VMAX (this also allows mid-ramp VMAX changes)
|
||||
linearRamp->targetVelocity = (linearRamp->targetPosition > linearRamp->rampPosition) ? linearRamp->maxVelocity : -linearRamp->maxVelocity;
|
||||
}
|
||||
break;
|
||||
case TMC_RAMP_LINEAR_STATE_BRAKING:
|
||||
if(linearRamp->targetPosition == linearRamp->rampPosition)
|
||||
{
|
||||
if(abs(linearRamp->rampVelocity) <= linearRamp->stopVelocity)
|
||||
{ // Position reached, velocity within cutoff threshold (or zero)
|
||||
linearRamp->rampVelocity = 0;
|
||||
linearRamp->targetVelocity = 0;
|
||||
linearRamp->state = TMC_RAMP_LINEAR_STATE_IDLE;
|
||||
}
|
||||
else
|
||||
{
|
||||
// We're still too fast, we're going to miss the target position
|
||||
// Let the deceleration continue until velocity is zero, then either
|
||||
// home when within homing distance or start a new ramp (RAMP_DRIVING)
|
||||
// towards the target.
|
||||
}
|
||||
}
|
||||
else
|
||||
{ // We're not at the target position
|
||||
if(linearRamp->rampVelocity != 0)
|
||||
{ // Still decelerating
|
||||
|
||||
// Calculate distance to target (positive = driving towards target)
|
||||
if(linearRamp->rampVelocity > 0)
|
||||
diffx = linearRamp->targetPosition - linearRamp->rampPosition;
|
||||
else if(linearRamp->rampVelocity < 0)
|
||||
diffx = -(linearRamp->targetPosition - linearRamp->rampPosition);
|
||||
else
|
||||
diffx = abs(linearRamp->targetPosition - linearRamp->rampPosition);
|
||||
|
||||
// Enough space to accelerate again?
|
||||
// (+ 1 to compensate rounding (flooring) errors of the position accumulator)
|
||||
if(linearRamp->accelerationSteps + 1 < diffx)
|
||||
{
|
||||
linearRamp->state = TMC_RAMP_LINEAR_STATE_DRIVING;
|
||||
}
|
||||
}
|
||||
else
|
||||
{ // Standing still (not at the target position)
|
||||
if(abs(linearRamp->targetPosition - linearRamp->rampPosition) <= linearRamp->homingDistance)
|
||||
{ // Within homing distance - drive with stop velocity
|
||||
linearRamp->targetVelocity = (linearRamp->targetPosition > linearRamp->rampPosition)? linearRamp->stopVelocity : -linearRamp->stopVelocity;
|
||||
}
|
||||
else
|
||||
{ // Not within homing distance - start a new motion by switching to RAMP_IDLE
|
||||
// Since (targetPosition != actualPosition) a new ramp will be started.
|
||||
linearRamp->state = TMC_RAMP_LINEAR_STATE_IDLE;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
89
TMC2209/lib/tmc/ramp/LinearRamp1.h
Normal file
89
TMC2209/lib/tmc/ramp/LinearRamp1.h
Normal file
@@ -0,0 +1,89 @@
|
||||
/*******************************************************************************
|
||||
* 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.
|
||||
*******************************************************************************/
|
||||
|
||||
|
||||
#ifndef TMC_RAMP_LINEARRAMP1_H_
|
||||
#define TMC_RAMP_LINEARRAMP1_H_
|
||||
|
||||
#include "tmc/helpers/API_Header.h"
|
||||
#include "Ramp.h"
|
||||
|
||||
// Default precision of the calculations. Internal calculations use a precision
|
||||
// of 1/TMC_RAMP_LINEAR_PRECISION for acceleration and velocity.
|
||||
// When using 2**N as precision, this results in N digits of precision.
|
||||
#define TMC_RAMP_LINEAR_DEFAULT_PRECISION ((uint32_t)1<<17)
|
||||
|
||||
// Position mode: When hitting the target position a velocity below the V_STOP threshold will be cut off to velocity 0
|
||||
#define TMC_RAMP_LINEAR_DEFAULT_HOMING_DISTANCE 5
|
||||
|
||||
// Position mode: When barely missing the target position by HOMING_DISTANCE or less, the remainder will be driven with V_STOP velocity
|
||||
#define TMC_RAMP_LINEAR_DEFAULT_STOP_VELOCITY 5
|
||||
|
||||
typedef enum {
|
||||
TMC_RAMP_LINEAR_MODE_VELOCITY,
|
||||
TMC_RAMP_LINEAR_MODE_POSITION
|
||||
} TMC_LinearRamp_Mode;
|
||||
|
||||
typedef enum {
|
||||
TMC_RAMP_LINEAR_STATE_IDLE,
|
||||
TMC_RAMP_LINEAR_STATE_DRIVING,
|
||||
TMC_RAMP_LINEAR_STATE_BRAKING
|
||||
} TMC_LinearRamp_State;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint32_t maxVelocity;
|
||||
int32_t targetPosition;
|
||||
int32_t rampPosition;
|
||||
int32_t targetVelocity;
|
||||
int32_t rampVelocity;
|
||||
int32_t acceleration;
|
||||
bool rampEnabled;
|
||||
int32_t accumulatorVelocity;
|
||||
int32_t accumulatorPosition;
|
||||
TMC_LinearRamp_Mode rampMode;
|
||||
TMC_LinearRamp_State state;
|
||||
int32_t accelerationSteps;
|
||||
uint32_t precision;
|
||||
uint32_t homingDistance;
|
||||
uint32_t stopVelocity;
|
||||
} TMC_LinearRamp;
|
||||
|
||||
void tmc_ramp_linear_init(TMC_LinearRamp *linearRamp);
|
||||
int32_t tmc_ramp_linear_compute(TMC_LinearRamp *linearRamp);
|
||||
int32_t tmc_ramp_linear_compute_velocity(TMC_LinearRamp *linearRamp);
|
||||
void tmc_ramp_linear_compute_position(TMC_LinearRamp *linearRamp);
|
||||
|
||||
void tmc_ramp_linear_set_enabled(TMC_LinearRamp *linearRamp, bool enabled);
|
||||
void tmc_ramp_linear_set_maxVelocity(TMC_LinearRamp *linearRamp, uint32_t maxVelocity);
|
||||
void tmc_ramp_linear_set_targetPosition(TMC_LinearRamp *linearRamp, int32_t targetPosition);
|
||||
void tmc_ramp_linear_set_rampPosition(TMC_LinearRamp *linearRamp, int32_t rampPosition);
|
||||
void tmc_ramp_linear_set_targetVelocity(TMC_LinearRamp *linearRamp, int32_t targetVelocity);
|
||||
void tmc_ramp_linear_set_rampVelocity(TMC_LinearRamp *linearRamp, int32_t rampVelocity);
|
||||
void tmc_ramp_linear_set_acceleration(TMC_LinearRamp *linearRamp, int32_t acceleration);
|
||||
void tmc_ramp_linear_set_mode(TMC_LinearRamp *linearRamp, TMC_LinearRamp_Mode mode);
|
||||
void tmc_ramp_linear_set_precision(TMC_LinearRamp * linearRamp, uint32_t precision);
|
||||
void tmc_ramp_linear_set_homingDistance(TMC_LinearRamp *linearRamp, uint32_t homingDistance);
|
||||
void tmc_ramp_linear_set_stopVelocity(TMC_LinearRamp *linearRamp, uint32_t stopVelocity);
|
||||
|
||||
bool tmc_ramp_linear_get_enabled(TMC_LinearRamp *linearRamp);
|
||||
uint32_t tmc_ramp_linear_get_maxVelocity(TMC_LinearRamp *linearRamp);
|
||||
int32_t tmc_ramp_linear_get_targetPosition(TMC_LinearRamp *linearRamp);
|
||||
int32_t tmc_ramp_linear_get_rampPosition(TMC_LinearRamp *linearRamp);
|
||||
int32_t tmc_ramp_linear_get_targetVelocity(TMC_LinearRamp *linearRamp);
|
||||
int32_t tmc_ramp_linear_get_rampVelocity(TMC_LinearRamp *linearRamp);
|
||||
int32_t tmc_ramp_linear_get_acceleration(TMC_LinearRamp *linearRamp);
|
||||
TMC_LinearRamp_State tmc_ramp_linear_get_state(TMC_LinearRamp *linearRamp);
|
||||
TMC_LinearRamp_Mode tmc_ramp_linear_get_mode(TMC_LinearRamp *linearRamp);
|
||||
uint32_t tmc_ramp_linear_get_precision(TMC_LinearRamp *linearRamp);
|
||||
uint32_t tmc_ramp_linear_get_acceleration_limit(TMC_LinearRamp *linearRamp);
|
||||
uint32_t tmc_ramp_linear_get_velocity_limit(TMC_LinearRamp *linearRamp);
|
||||
uint32_t tmc_ramp_linear_get_homingDistance(TMC_LinearRamp *linearRamp);
|
||||
uint32_t tmc_ramp_linear_get_stopVelocity(TMC_LinearRamp *linearRamp);
|
||||
|
||||
#endif /* TMC_RAMP_LINEARRAMP1_H_ */
|
||||
91
TMC2209/lib/tmc/ramp/Ramp.c
Normal file
91
TMC2209/lib/tmc/ramp/Ramp.c
Normal file
@@ -0,0 +1,91 @@
|
||||
/*******************************************************************************
|
||||
* 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.
|
||||
*******************************************************************************/
|
||||
|
||||
|
||||
#include "Ramp.h"
|
||||
|
||||
void tmc_ramp_init(void *ramp, TMC_RampType type)
|
||||
{
|
||||
switch(type) {
|
||||
case TMC_RAMP_TYPE_LINEAR:
|
||||
default:
|
||||
tmc_ramp_linear_init((TMC_LinearRamp *)ramp);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int32_t tmc_ramp_compute(void *ramp, TMC_RampType type, uint32_t delta)
|
||||
{
|
||||
uint32_t i;
|
||||
int32_t dxSum = 0;
|
||||
|
||||
switch(type) {
|
||||
case TMC_RAMP_TYPE_LINEAR:
|
||||
default:
|
||||
for (i = 0; i < delta; i++)
|
||||
{
|
||||
dxSum += tmc_ramp_linear_compute((TMC_LinearRamp *)ramp);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
return dxSum;
|
||||
}
|
||||
|
||||
int32_t tmc_ramp_get_rampVelocity(void *ramp, TMC_RampType type)
|
||||
{
|
||||
int32_t v = 0;
|
||||
switch(type) {
|
||||
case TMC_RAMP_TYPE_LINEAR:
|
||||
v = tmc_ramp_linear_get_rampVelocity((TMC_LinearRamp *)ramp);
|
||||
break;
|
||||
}
|
||||
return v;
|
||||
}
|
||||
|
||||
int32_t tmc_ramp_get_rampPosition(void *ramp, TMC_RampType type)
|
||||
{
|
||||
int32_t x = 0;
|
||||
switch(type) {
|
||||
case TMC_RAMP_TYPE_LINEAR:
|
||||
x = tmc_ramp_linear_get_rampPosition((TMC_LinearRamp *)ramp);
|
||||
break;
|
||||
}
|
||||
return x;
|
||||
}
|
||||
|
||||
bool tmc_ramp_get_enabled(void *ramp, TMC_RampType type)
|
||||
{
|
||||
bool enabled = false;
|
||||
switch(type) {
|
||||
case TMC_RAMP_TYPE_LINEAR:
|
||||
enabled = tmc_ramp_linear_get_enabled((TMC_LinearRamp *)ramp);
|
||||
break;
|
||||
}
|
||||
return enabled;
|
||||
}
|
||||
|
||||
void tmc_ramp_set_enabled(void *ramp, TMC_RampType type, bool enabled)
|
||||
{
|
||||
switch(type) {
|
||||
case TMC_RAMP_TYPE_LINEAR:
|
||||
default:
|
||||
tmc_ramp_linear_set_enabled((TMC_LinearRamp *)ramp, enabled);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void tmc_ramp_toggle_enabled(void *ramp, TMC_RampType type)
|
||||
{
|
||||
switch(type) {
|
||||
case TMC_RAMP_TYPE_LINEAR:
|
||||
default:
|
||||
tmc_ramp_linear_set_enabled((TMC_LinearRamp *)ramp, !tmc_ramp_get_enabled(ramp, type));
|
||||
break;
|
||||
}
|
||||
}
|
||||
40
TMC2209/lib/tmc/ramp/Ramp.h
Normal file
40
TMC2209/lib/tmc/ramp/Ramp.h
Normal file
@@ -0,0 +1,40 @@
|
||||
/*******************************************************************************
|
||||
* 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.
|
||||
*******************************************************************************/
|
||||
|
||||
|
||||
#ifndef TMC_RAMP_RAMP_H_
|
||||
#define TMC_RAMP_RAMP_H_
|
||||
|
||||
#include "LinearRamp1.h"
|
||||
|
||||
typedef enum {
|
||||
TMC_RAMP_TYPE_LINEAR
|
||||
} TMC_RampType;
|
||||
|
||||
// Initializes ramp parameters for given type
|
||||
void tmc_ramp_init(void *ramp, TMC_RampType type);
|
||||
|
||||
// Computes new ramp state after delta ticks have passed
|
||||
// Note: To call this function periodically with a fixed delta-time, use delta = 1 and
|
||||
// define the units of acceleration as v/delta-time. If you want to specify a different unit,
|
||||
// change delta to your preference.
|
||||
// Returns the position difference of the calculation.
|
||||
int32_t tmc_ramp_compute(void *ramp, TMC_RampType type, uint32_t delta);
|
||||
|
||||
// Returns the current ramp velocity computed by the given ramp
|
||||
int32_t tmc_ramp_get_rampVelocity(void *ramp, TMC_RampType type);
|
||||
|
||||
// Returns the current ramp position computed by the given ramp
|
||||
int32_t tmc_ramp_get_rampPosition(void *ramp, TMC_RampType type);
|
||||
|
||||
// Enable/disable ramps
|
||||
bool tmc_ramp_get_enabled(void *ramp, TMC_RampType type);
|
||||
void tmc_ramp_set_enabled(void *ramp, TMC_RampType type, bool enabled);
|
||||
void tmc_ramp_toggle_enabled(void *ramp, TMC_RampType type);
|
||||
|
||||
#endif /* TMC_RAMP_RAMP_H_ */
|
||||
Reference in New Issue
Block a user