BeRTOS
|
Compute, save and load ramps for stepper motors. More...
Go to the source code of this file.
Data Structures | |
struct | RampPrecalc |
Structure holding pre-calculated data for speeding up real-time evaluation of the ramp. More... | |
struct | Ramp |
Ramp structure. More... | |
Defines | |
#define | TIME2CLOCKS(micros) ((uint32_t)(micros) * (STEPPER_CLOCK / 1000000)) |
Convert microseconds to timer clock ticks. | |
#define | CLOCKS2TIME(clocks) ((uint32_t)(clocks) / (STEPPER_CLOCK / 1000000)) |
Convert timer clock ticks back to microseconds. | |
#define | MICROS2FREQ(micros) (1000000UL / ((uint32_t)(micros))) |
Convert microseconds to Hz. | |
#define | FREQ2MICROS(hz) (1000000UL / ((uint32_t)(hz))) |
Convert frequency (in Hz) to time (in microseconds) | |
#define | FIX_MULT32(a, b) (((uint64_t)(a)*(uint32_t)(b)) >> 16) |
Multiply a and b two integer at 32 bit and extract the high 16 bit word. | |
Functions | |
void | ramp_setup (struct Ramp *ramp, uint32_t length, uint32_t minFreq, uint32_t maxFreq) |
Setup an acceleration ramp for a stepper motor. | |
void | ramp_default (struct Ramp *ramp) |
Initialize a new ramp with default values. | |
uint16_t | ramp_evaluate (const struct Ramp *ramp, uint32_t curClock) |
Evaluate the ramp at the given point. | |
int | ramp_testSetup (void) |
Self test. |
Compute, save and load ramps for stepper motors.
The acceleration ramp is used to properly accelerate a stepper motor. The main entry point is the function ramp_evaluate(), which must be called at every step of the motor: it gets as input the time elapsed since the stepper started accelerating, and returns the time to wait before sending the next step. A pseudo usage pattern is as follows:
float time = 0; while (1) { float delta = ramp_evaluate(&my_ramp, time); sleep(delta); do_motor_step(); time += delta; }
A similar pattern can be used to decelerate (it is sufficient to move the total time backward, such as "time -= delta").
The ramp can be configured with ramp_setup(), providing it with the minimum and maximum operating frequency of the motor, and the total acceleration time in milliseconds (that is, the time that will be needed to accelerate from the minimum frequency to the maximum frequency).
Both a very precise floating point and a very fast fixed point implementation of the ramp evaluation are provided. The fixed point is hand-optimized assembly for DSP56000 (but a portable C version of it can be easily written, see the comments in the code).
Definition in file ramp.h.
uint16_t ramp_evaluate | ( | const struct Ramp * | ramp, |
uint32_t | curClock | ||
) |
Evaluate the ramp at the given point.
Given a ramp, and the current clock since the start of the acceleration, compute the next step, that is the interval at which send the signal to the motor.
void ramp_setup | ( | struct Ramp * | ramp, |
uint32_t | length, | ||
uint32_t | minFreq, | ||
uint32_t | maxFreq | ||
) |