68 lines
2.3 KiB
C++
68 lines
2.3 KiB
C++
/// @file speed_table.h
|
|
#pragma once
|
|
#include "../config/config.h"
|
|
#include "../hal/progmem.h"
|
|
#include "../hal/cpu.h"
|
|
#include "math.h"
|
|
|
|
namespace modules {
|
|
|
|
/// Speed tables for acceleration calculations
|
|
namespace speed_table {
|
|
|
|
typedef uint16_t st_timer_t;
|
|
|
|
/// CPU timer frequency divider required for the speed tables
|
|
static_assert(F_CPU / config::stepTimerFrequencyDivider == 2000000,
|
|
"speed tables not compatible for the requested frequency");
|
|
|
|
/// Lookup table for rates equal or higher than 8*256
|
|
extern const st_timer_t speed_table_fast[256][2] PROGMEM;
|
|
|
|
/// Lookup table for lower step rates
|
|
extern const st_timer_t speed_table_slow[256][2] PROGMEM;
|
|
|
|
/// Calculate the next timer interval and steps according to current step rate
|
|
static inline st_timer_t calc_timer(st_timer_t step_rate, uint8_t &step_loops) {
|
|
if (step_rate > config::maxStepFrequency)
|
|
step_rate = config::maxStepFrequency;
|
|
if (step_rate > 20000) { // If steprate > 20kHz >> step 4 times
|
|
step_rate = (step_rate >> 2) & 0x3fff;
|
|
step_loops = 4;
|
|
} else if (step_rate > 10000) { // If steprate > 10kHz >> step 2 times
|
|
step_rate = (step_rate >> 1) & 0x7fff;
|
|
step_loops = 2;
|
|
} else {
|
|
step_loops = 1;
|
|
}
|
|
|
|
using modules::math::mulU8X16toH16;
|
|
namespace pm = hal::progmem;
|
|
|
|
st_timer_t timer; // calculated interval
|
|
|
|
if (step_rate < (F_CPU / 500000))
|
|
step_rate = (F_CPU / 500000);
|
|
step_rate -= (F_CPU / 500000); // Correct for minimal speed
|
|
if (step_rate >= (8 * 256)) { // higher step rate
|
|
const uint16_t *table_address = &speed_table_fast[(uint8_t)(step_rate >> 8)][0];
|
|
uint8_t tmp_step_rate = (step_rate & 0x00ff);
|
|
uint16_t gain = pm::read_word(table_address + 1);
|
|
timer = mulU8X16toH16(tmp_step_rate, gain);
|
|
timer = pm::read_word(table_address) - timer;
|
|
} else { // lower step rates
|
|
const uint16_t *table_address = &speed_table_slow[0][0];
|
|
table_address += (step_rate >> 2) & 0xfffe;
|
|
timer = pm::read_word(table_address);
|
|
timer -= ((pm::read_word(table_address + 1) * (uint8_t)(step_rate & 0x0007)) >> 3);
|
|
}
|
|
if (timer < 100) {
|
|
// 20kHz this should never happen
|
|
timer = 100;
|
|
}
|
|
return timer;
|
|
}
|
|
|
|
} // namespace speed_table
|
|
} // namespace modules
|