first
This commit is contained in:
80
src/AVRStepperPins.h
Normal file
80
src/AVRStepperPins.h
Normal file
@@ -0,0 +1,80 @@
|
||||
#ifndef AVRSTEPPERPINS_H
|
||||
#define AVRSTEPPERPINS_H
|
||||
|
||||
#if defined(ARDUINO_ARCH_AVR)
|
||||
#include <Arduino.h>
|
||||
|
||||
/** * Warning: Other libraries may also use the timers!
|
||||
*
|
||||
* For example Serial library and delay() functions for example.
|
||||
* Using the same timer may cause strange effects, you are best to avoid using
|
||||
* those other libraries at the some time or use a different pin where
|
||||
* possible!
|
||||
*/
|
||||
|
||||
#if !(defined(__AVR_ATmega168__) || defined(__AVR_ATmega168P__) || \
|
||||
defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__) || \
|
||||
defined(__AVR_ATmega2560__) || defined(__AVR_ATmega32U4__))
|
||||
#error "Unsupported AVR derivate"
|
||||
#endif
|
||||
|
||||
// The ATmega328P has one 16 bit timer: Timer 1
|
||||
// The ATmega2560 has four 16 bit timers: Timer 1, 3, 4 and 5
|
||||
#if (defined(__AVR_ATmega168__) || defined(__AVR_ATmega168P__) || \
|
||||
defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__))
|
||||
#define FAS_TIMER_MODULE 1
|
||||
#define stepPinStepper1A 9 /* OC1A */
|
||||
#define stepPinStepper1B 10 /* OC1B */
|
||||
#elif defined(__AVR_ATmega2560__)
|
||||
#ifndef FAS_TIMER_MODULE
|
||||
#define FAS_TIMER_MODULE 4
|
||||
#endif
|
||||
#define stepPinStepper1A 11 /* OC1A */
|
||||
#define stepPinStepper1B 12 /* OC1B */
|
||||
#define stepPinStepper1C 13 /* OC1C */
|
||||
#define stepPinStepper3A 5 /* OC3A */
|
||||
#define stepPinStepper3B 2 /* OC3B */
|
||||
#define stepPinStepper3C 3 /* OC3C */
|
||||
#define stepPinStepper4A 6 /* OC4A */
|
||||
#define stepPinStepper4B 7 /* OC4B */
|
||||
#define stepPinStepper4C 8 /* OC4C */
|
||||
#define stepPinStepper5A 46 /* OC5A */
|
||||
#define stepPinStepper5B 45 /* OC5B */
|
||||
#define stepPinStepper5C 44 /* OC5C */
|
||||
|
||||
#elif defined(__AVR_ATmega32U4__)
|
||||
#define FAS_TIMER_MODULE 1
|
||||
#define stepPinStepper1A 9 /* OC1A */
|
||||
#define stepPinStepper1B 10 /* OC1B */
|
||||
#define stepPinStepper1C 11 /* OC1C */
|
||||
#endif
|
||||
|
||||
#if (FAS_TIMER_MODULE == 1)
|
||||
#define stepPinStepperA stepPinStepper1A
|
||||
#define stepPinStepperB stepPinStepper1B
|
||||
#if defined(__AVR_ATmega2560__) || defined(__AVR_ATmega32U4__)
|
||||
#define stepPinStepperC stepPinStepper1C
|
||||
#endif
|
||||
#elif (FAS_TIMER_MODULE == 3)
|
||||
#define stepPinStepperA stepPinStepper3A
|
||||
#define stepPinStepperB stepPinStepper3B
|
||||
#if defined(__AVR_ATmega2560__)
|
||||
#define stepPinStepperC stepPinStepper3C
|
||||
#endif
|
||||
#elif (FAS_TIMER_MODULE == 4)
|
||||
#define stepPinStepperA stepPinStepper4A
|
||||
#define stepPinStepperB stepPinStepper4B
|
||||
#if defined(__AVR_ATmega2560__)
|
||||
#define stepPinStepperC stepPinStepper4C
|
||||
#endif
|
||||
#elif (FAS_TIMER_MODULE == 5)
|
||||
#define stepPinStepperA stepPinStepper5A
|
||||
#define stepPinStepperB stepPinStepper5B
|
||||
#if defined(__AVR_ATmega2560__)
|
||||
#define stepPinStepperC stepPinStepper5C
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#endif // ARDUINO_ARCH_AVR
|
||||
|
||||
#endif // AVRSTEPPERPINS_H
|
||||
906
src/FastAccelStepper.cpp
Normal file
906
src/FastAccelStepper.cpp
Normal file
@@ -0,0 +1,906 @@
|
||||
#include "FastAccelStepper.h"
|
||||
#include "StepperISR.h"
|
||||
|
||||
// This define in order to not shoot myself.
|
||||
#ifndef TEST
|
||||
#define printf DO_NOT_USE_PRINTF
|
||||
#define puts DO_NOT_USE_PUTS
|
||||
#endif
|
||||
|
||||
// Here are the global variables to interface with the interrupts
|
||||
|
||||
// To realize the 1 Hz debug led
|
||||
static uint8_t fas_ledPin = PIN_UNDEFINED;
|
||||
static uint16_t fas_debug_led_cnt = 0;
|
||||
|
||||
// dynamic allocation seems to not work so well on avr
|
||||
FastAccelStepper fas_stepper[MAX_STEPPER];
|
||||
|
||||
//*************************************************************************************************
|
||||
//*************************************************************************************************
|
||||
void FastAccelStepperEngine::init() {
|
||||
_externalCallForPin = nullptr;
|
||||
_stepper_cnt = 0;
|
||||
fas_init_engine(this, 255);
|
||||
for (auto & i : _stepper) {
|
||||
i = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(SUPPORT_CPU_AFFINITY)
|
||||
void FastAccelStepperEngine::init(uint8_t cpu_core) {
|
||||
_externalCallForPin = nullptr;
|
||||
_stepper_cnt = 0;
|
||||
fas_init_engine(this, cpu_core);
|
||||
}
|
||||
#endif
|
||||
void FastAccelStepperEngine::setExternalCallForPin(
|
||||
bool (*func)(uint8_t pin, uint8_t value)) {
|
||||
_externalCallForPin = func;
|
||||
}
|
||||
//*************************************************************************************************
|
||||
bool FastAccelStepperEngine::_isValidStepPin(uint8_t step_pin) {
|
||||
return StepperQueue::isValidStepPin(step_pin);
|
||||
}
|
||||
//*************************************************************************************************
|
||||
bool FastAccelStepperEngine::isDirPinBusy(uint8_t dir_pin,
|
||||
uint8_t except_stepper) {
|
||||
for (uint8_t i = 0; i < MAX_STEPPER; i++) {
|
||||
if (i != except_stepper) {
|
||||
FastAccelStepper* s = _stepper[i];
|
||||
if (s) {
|
||||
if (s->getDirectionPin() == dir_pin) {
|
||||
if (s->isQueueRunning()) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
//*************************************************************************************************
|
||||
#if !defined(SUPPORT_SELECT_DRIVER_TYPE)
|
||||
FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin(uint8_t step_pin)
|
||||
#else
|
||||
FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin(
|
||||
uint8_t step_pin, uint8_t driver_type)
|
||||
#endif
|
||||
{
|
||||
// Check if already connected
|
||||
for (auto s : _stepper) {
|
||||
if (s) {
|
||||
if (s->getStepPin() == step_pin) {
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (!_isValidStepPin(step_pin)) {
|
||||
return nullptr;
|
||||
}
|
||||
#if !defined(SUPPORT_SELECT_DRIVER_TYPE)
|
||||
int8_t fas_stepper_num = StepperQueue::queueNumForStepPin(step_pin);
|
||||
if (fas_stepper_num < 0) { // flexible, so just choose next
|
||||
if (_stepper_cnt >= MAX_STEPPER) {
|
||||
return nullptr;
|
||||
}
|
||||
fas_stepper_num = _stepper_cnt;
|
||||
}
|
||||
#else
|
||||
uint8_t queue_from = 0;
|
||||
uint8_t queue_to = QUEUES_MCPWM_PCNT + QUEUES_RMT;
|
||||
if (driver_type == DRIVER_MCPWM_PCNT) {
|
||||
queue_to = QUEUES_MCPWM_PCNT;
|
||||
} else if (driver_type == DRIVER_RMT) {
|
||||
queue_from = QUEUES_MCPWM_PCNT;
|
||||
}
|
||||
int8_t fas_stepper_num = -1;
|
||||
for (uint8_t i = queue_from; i < queue_to; i++) {
|
||||
FastAccelStepper* s = _stepper[i];
|
||||
if (s == NULL) {
|
||||
fas_stepper_num = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (fas_stepper_num < 0) {
|
||||
return NULL;
|
||||
}
|
||||
#endif
|
||||
_stepper_cnt++;
|
||||
|
||||
FastAccelStepper* s = &fas_stepper[fas_stepper_num];
|
||||
_stepper[fas_stepper_num] = s;
|
||||
s->init(this, fas_stepper_num, step_pin);
|
||||
for (auto sx : _stepper) {
|
||||
if (sx) {
|
||||
fas_queue[sx->_queue_num].adjustSpeedToStepperCount(_stepper_cnt);
|
||||
}
|
||||
}
|
||||
return s;
|
||||
}
|
||||
//*************************************************************************************************
|
||||
void FastAccelStepperEngine::setDebugLed(uint8_t ledPin) {
|
||||
fas_ledPin = ledPin;
|
||||
PIN_OUTPUT(fas_ledPin, LOW);
|
||||
}
|
||||
//*************************************************************************************************
|
||||
void FastAccelStepperEngine::manageSteppers() {
|
||||
#ifdef DEBUG_LED_HALF_PERIOD
|
||||
if (fas_ledPin != PIN_UNDEFINED) {
|
||||
fas_debug_led_cnt++;
|
||||
if (fas_debug_led_cnt == DEBUG_LED_HALF_PERIOD) {
|
||||
digitalWrite(fas_ledPin, HIGH);
|
||||
}
|
||||
if (fas_debug_led_cnt == 2 * DEBUG_LED_HALF_PERIOD) {
|
||||
digitalWrite(fas_ledPin, LOW);
|
||||
fas_debug_led_cnt = 0;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
for (auto s : _stepper) {
|
||||
if (s) {
|
||||
#ifdef SUPPORT_EXTERNAL_DIRECTION_PIN
|
||||
if (s->externalDirPinChangeCompletedIfNeeded()) {
|
||||
s->fill_queue();
|
||||
}
|
||||
#else
|
||||
s->fill_queue();
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
// Check for auto disable
|
||||
for (uint8_t i = 0; i < MAX_STEPPER; i++) {
|
||||
FastAccelStepper* s = _stepper[i];
|
||||
if (s) {
|
||||
if (s->needAutoDisable()) {
|
||||
uint8_t high_active_pin = s->getEnablePinHighActive();
|
||||
uint8_t low_active_pin = s->getEnablePinLowActive();
|
||||
|
||||
// fasDisableInterrupts(); // TODO
|
||||
bool agree = true;
|
||||
for (uint8_t j = 0; j < MAX_STEPPER; j++) {
|
||||
if (i != j) {
|
||||
FastAccelStepper* other = _stepper[j];
|
||||
if (other) {
|
||||
if (other->usesAutoEnablePin(high_active_pin) ||
|
||||
other->usesAutoEnablePin(low_active_pin)) {
|
||||
if (!other->agreeWithAutoDisable()) {
|
||||
agree = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (agree) {
|
||||
for (auto current : _stepper) {
|
||||
if (current) {
|
||||
if (current->usesAutoEnablePin(high_active_pin) ||
|
||||
current->usesAutoEnablePin(low_active_pin)) {
|
||||
// if successful, then the _auto_disable_delay_counter is zero
|
||||
// Otherwise in next loop will be checked for auto disable again
|
||||
current->disableOutputs();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// fasEnableInterrupts();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Update the auto disable counters
|
||||
for (auto s : _stepper) {
|
||||
if (s) {
|
||||
fasDisableInterrupts();
|
||||
// update the counters down to 1
|
||||
s->updateAutoDisable();
|
||||
fasEnableInterrupts();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//*************************************************************************************************
|
||||
//*************************************************************************************************
|
||||
//
|
||||
// FastAccelStepper provides:
|
||||
// - movement control
|
||||
// either raw access to the stepper command queue
|
||||
// or ramp generator driven by speed/acceleration and move
|
||||
// - stepper position
|
||||
//
|
||||
// This implements auto enable and delay from direction change to first step
|
||||
//
|
||||
//*************************************************************************************************
|
||||
//*************************************************************************************************
|
||||
|
||||
//*************************************************************************************************
|
||||
int8_t FastAccelStepper::addQueueEntry(const struct stepper_command_s* cmd,
|
||||
bool start) {
|
||||
StepperQueue* q = &fas_queue[_queue_num];
|
||||
if (cmd == nullptr) {
|
||||
return q->addQueueEntry(nullptr, start);
|
||||
}
|
||||
if (cmd->ticks < q->max_speed_in_ticks) {
|
||||
return AQE_ERROR_TICKS_TOO_LOW;
|
||||
}
|
||||
|
||||
if (_dirPin != PIN_UNDEFINED) {
|
||||
if (!isQueueRunning()) {
|
||||
if (_engine != nullptr) {
|
||||
if (_engine->isDirPinBusy(_dirPin, _queue_num)) {
|
||||
return AQE_DIR_PIN_IS_BUSY;
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
if (!cmd->count_up) {
|
||||
return AQE_ERROR_NO_DIR_PIN_TO_TOGGLE;
|
||||
}
|
||||
}
|
||||
|
||||
int res = AQE_OK;
|
||||
if (_autoEnable) {
|
||||
fasDisableInterrupts();
|
||||
uint16_t delay_counter = _auto_disable_delay_counter;
|
||||
fasEnableInterrupts();
|
||||
if (delay_counter == 0) {
|
||||
// outputs are disabled
|
||||
if (!enableOutputs()) {
|
||||
return AQE_WAIT_FOR_ENABLE_PIN_ACTIVE;
|
||||
}
|
||||
// if on delay is defined, fill queue if required amount of pauses before
|
||||
// the first step
|
||||
if (_on_delay_ticks > 0) {
|
||||
uint32_t delay = _on_delay_ticks;
|
||||
// this delay sets count_up appropriately. If this is shorter than
|
||||
// dir_change_delay_ticks, then extend accordingly
|
||||
if ((delay < _dir_change_delay_ticks) &&
|
||||
(q->queue_end.count_up != cmd->count_up)) {
|
||||
delay = _dir_change_delay_ticks;
|
||||
}
|
||||
while (delay > 0) {
|
||||
uint32_t ticks = delay >> 1;
|
||||
uint16_t ticks_u16 = ticks;
|
||||
if (ticks > 65535) {
|
||||
ticks_u16 = 65535;
|
||||
} else if (ticks < 32768) {
|
||||
ticks_u16 = delay;
|
||||
}
|
||||
struct stepper_command_s start_cmd = {
|
||||
.ticks = ticks_u16, .steps = 0, .count_up = cmd->count_up};
|
||||
q->addQueueEntry(&start_cmd, false);
|
||||
delay -= ticks_u16;
|
||||
}
|
||||
res = q->addQueueEntry(nullptr, start);
|
||||
if (res != AQE_OK) {
|
||||
return res;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (q->queue_end.count_up != cmd->count_up) {
|
||||
// Change of direction has been detected.
|
||||
if (_dirPin & PIN_EXTERNAL_FLAG) {
|
||||
// for external pins, two pause commands need to be added. The first one
|
||||
// with the dir pin change. The second one just a pause.
|
||||
// The queue's addQueueEntry() will set repeat_entry for the command entry
|
||||
if (q->queueEntries() > QUEUE_LEN - 2) {
|
||||
// no space for two commands => do nothing and return QUEUE_FULL
|
||||
return AQE_QUEUE_FULL;
|
||||
}
|
||||
struct stepper_command_s start_cmd = {
|
||||
.ticks = US_TO_TICKS(500), .steps = 0, .count_up = cmd->count_up};
|
||||
res = q->addQueueEntry(&start_cmd, start);
|
||||
if (res != AQE_OK) {
|
||||
return res;
|
||||
}
|
||||
res = q->addQueueEntry(&start_cmd, start);
|
||||
if (res != AQE_OK) {
|
||||
return res;
|
||||
}
|
||||
} else if ((_dir_change_delay_ticks != 0) && (cmd->steps != 0)) {
|
||||
// add pause command to delay dir pin change to first step
|
||||
struct stepper_command_s start_cmd = {.ticks = _dir_change_delay_ticks,
|
||||
.steps = 0,
|
||||
.count_up = cmd->count_up};
|
||||
res = q->addQueueEntry(&start_cmd, start);
|
||||
if (res != AQE_OK) {
|
||||
return res;
|
||||
}
|
||||
}
|
||||
}
|
||||
res = q->addQueueEntry(cmd, start);
|
||||
if (_autoEnable) {
|
||||
if (res == AQE_OK) {
|
||||
fasDisableInterrupts();
|
||||
_auto_disable_delay_counter = _off_delay_count;
|
||||
fasEnableInterrupts();
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
#ifdef SUPPORT_EXTERNAL_DIRECTION_PIN
|
||||
bool FastAccelStepper::externalDirPinChangeCompletedIfNeeded() {
|
||||
StepperQueue* q = &fas_queue[_queue_num];
|
||||
if ((_dirPin != PIN_UNDEFINED) && ((_dirPin & PIN_EXTERNAL_FLAG) != 0)) {
|
||||
if (q->isOnRepeatingEntry()) {
|
||||
if (_engine->_externalCallForPin) {
|
||||
uint8_t state = q->dirPinState();
|
||||
bool newState = _engine->_externalCallForPin(_dirPin, state);
|
||||
if (newState != state) {
|
||||
return false;
|
||||
}
|
||||
q->clearRepeatingFlag();
|
||||
}
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
||||
//*************************************************************************************************
|
||||
// fill_queue generates commands to the stepper for executing a ramp
|
||||
//
|
||||
// Plan is to fill the queue with commmands summing up to approx. 10 ms in the
|
||||
// future (or more). For low speeds, this results in single stepping For high
|
||||
// speeds (40kSteps/s) approx. 400 Steps to be created using 3 commands
|
||||
//
|
||||
//*************************************************************************************************
|
||||
|
||||
void FastAccelStepper::fill_queue() {
|
||||
// Check preconditions to be allowed to fill the queue
|
||||
if (!_rg.isRampGeneratorActive()) {
|
||||
return;
|
||||
}
|
||||
if (!_rg.hasValidConfig()) {
|
||||
#ifdef TEST
|
||||
assert(false);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
// check if addition of commands is suspended (due to forceStopAndNewPosition)
|
||||
StepperQueue* q = &fas_queue[_queue_num];
|
||||
// if force stop has been called, then ignore_commands is true and ramp
|
||||
// stopped. So the ramp generator will not create a new command, unless new
|
||||
// move command has been given after forceStop..(). So we just clear the flag
|
||||
q->ignore_commands = false;
|
||||
|
||||
// preconditions are fulfilled, so create the command(s)
|
||||
NextCommand cmd;
|
||||
// Plan ahead for max. 20 ms and minimum two commands.
|
||||
// This is now configurable using _forward_planning_in_ticks.
|
||||
bool delayed_start = !q->isRunning();
|
||||
bool need_delayed_start = false;
|
||||
uint32_t ticksPrepared = q->ticksInQueue();
|
||||
while (!isQueueFull() &&
|
||||
((ticksPrepared < _forward_planning_in_ticks) ||
|
||||
(q->queueEntries() <= 1)) &&
|
||||
_rg.isRampGeneratorActive()) {
|
||||
#if (TEST_MEASURE_ISR_SINGLE_FILL == 1)
|
||||
// For run time measurement
|
||||
uint32_t runtime_us = micros();
|
||||
#endif
|
||||
int8_t res = AQE_OK;
|
||||
_rg.getNextCommand(&q->queue_end, &cmd);
|
||||
if (cmd.command.ticks != 0) {
|
||||
res = addQueueEntry(&cmd.command, !delayed_start);
|
||||
}
|
||||
if (res == AQE_OK) {
|
||||
_rg.afterCommandEnqueued(&cmd);
|
||||
need_delayed_start = delayed_start;
|
||||
if (cmd.command.steps <= 1) {
|
||||
ticksPrepared += cmd.command.ticks;
|
||||
} else {
|
||||
uint32_t tmp = cmd.command.ticks;
|
||||
tmp *= cmd.command.steps;
|
||||
ticksPrepared += tmp;
|
||||
}
|
||||
}
|
||||
|
||||
#if (TEST_MEASURE_ISR_SINGLE_FILL == 1)
|
||||
// For run time measurement
|
||||
runtime_us = micros() - runtime_us;
|
||||
max_micros = fas_max(max_micros, runtime_us);
|
||||
#endif
|
||||
if (cmd.command.ticks == 0) {
|
||||
break;
|
||||
}
|
||||
if (res != AQE_OK) {
|
||||
if (res > 0) {
|
||||
// try later again
|
||||
break;
|
||||
} else {
|
||||
#ifdef SIM_TEST_INPUT
|
||||
Serial.println("Abort ramp due to queue error res=");
|
||||
Serial.print(res);
|
||||
Serial.print(" Steps=");
|
||||
Serial.print(cmd.command.steps);
|
||||
Serial.print(" ticks=");
|
||||
Serial.print(cmd.command.ticks);
|
||||
Serial.print(" min_cmd_ticks=");
|
||||
Serial.println(MIN_CMD_TICKS);
|
||||
#endif
|
||||
#ifdef TEST
|
||||
printf("ERROR: Abort ramp due to queue error (%d)\n", res);
|
||||
printf("steps=%d ticks=%d limit=%ld state=%d\n", cmd.command.steps,
|
||||
cmd.command.ticks, MIN_CMD_TICKS, cmd.rw.ramp_state);
|
||||
assert(false);
|
||||
#endif
|
||||
_rg.stopRamp();
|
||||
delayed_start = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (need_delayed_start) {
|
||||
addQueueEntry(nullptr, true);
|
||||
}
|
||||
}
|
||||
|
||||
void FastAccelStepper::updateAutoDisable() {
|
||||
// FastAccelStepperEngine will call with interrupts disabled
|
||||
// fasDisableInterrupts();
|
||||
if (_auto_disable_delay_counter > 1) {
|
||||
if (!isRunning()) {
|
||||
_auto_disable_delay_counter--;
|
||||
}
|
||||
}
|
||||
// fasEnableInterrupts();
|
||||
}
|
||||
|
||||
bool FastAccelStepper::agreeWithAutoDisable() {
|
||||
bool agree = true;
|
||||
// FastAccelStepperEngine will call with interrupts disabled
|
||||
// fasDisableInterrupts();
|
||||
if (isRunning()) {
|
||||
agree = false;
|
||||
}
|
||||
if (_auto_disable_delay_counter > 1) {
|
||||
agree = false;
|
||||
}
|
||||
// fasEnableInterrupts();
|
||||
return agree;
|
||||
}
|
||||
|
||||
bool FastAccelStepper::needAutoDisable() {
|
||||
bool need_disable = false;
|
||||
// FastAccelStepperEngine will call with interrupts disabled
|
||||
// fasDisableInterrupts();
|
||||
if (_auto_disable_delay_counter == 1) {
|
||||
if (!isRunning()) {
|
||||
need_disable = true;
|
||||
}
|
||||
}
|
||||
// fasEnableInterrupts();
|
||||
return need_disable;
|
||||
}
|
||||
|
||||
bool FastAccelStepper::usesAutoEnablePin(uint8_t pin) const {
|
||||
if (pin != PIN_UNDEFINED) {
|
||||
if ((pin == _enablePinHighActive) || (pin == _enablePinLowActive)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void FastAccelStepper::init(FastAccelStepperEngine* engine, uint8_t num,
|
||||
uint8_t step_pin) {
|
||||
#if (TEST_MEASURE_ISR_SINGLE_FILL == 1)
|
||||
// For run time measurement
|
||||
max_micros = 0;
|
||||
#endif
|
||||
_engine = engine;
|
||||
_autoEnable = false;
|
||||
_dir_change_delay_ticks = 0;
|
||||
_on_delay_ticks = 0;
|
||||
_off_delay_count = 1;
|
||||
_auto_disable_delay_counter = 0;
|
||||
_stepPin = step_pin;
|
||||
_dirHighCountsUp = true;
|
||||
_dirPin = PIN_UNDEFINED;
|
||||
_enablePinHighActive = PIN_UNDEFINED;
|
||||
_enablePinLowActive = PIN_UNDEFINED;
|
||||
_forward_planning_in_ticks = TICKS_PER_S / 50;
|
||||
_rg.init();
|
||||
|
||||
_queue_num = num;
|
||||
fas_queue[_queue_num].init(_queue_num, step_pin);
|
||||
#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 5)
|
||||
_attached_pulse_unit = nullptr;
|
||||
#endif
|
||||
#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 4)
|
||||
_attached_pulse_cnt_unit = -1;
|
||||
#endif
|
||||
}
|
||||
uint8_t FastAccelStepper::getStepPin() const { return _stepPin; }
|
||||
void FastAccelStepper::setDirectionPin(uint8_t dirPin, bool dirHighCountsUp,
|
||||
uint16_t dir_change_delay_us) {
|
||||
_dirPin = dirPin;
|
||||
_dirHighCountsUp = dirHighCountsUp;
|
||||
if (_dirPin != PIN_UNDEFINED) {
|
||||
if (_dirPin & PIN_EXTERNAL_FLAG) {
|
||||
if (_engine->_externalCallForPin) {
|
||||
_engine->_externalCallForPin(_dirPin, dirHighCountsUp ? HIGH : LOW);
|
||||
}
|
||||
} else {
|
||||
PIN_OUTPUT(dirPin, dirHighCountsUp ? HIGH : LOW);
|
||||
}
|
||||
}
|
||||
fas_queue[_queue_num].setDirPin(dirPin, dirHighCountsUp);
|
||||
if (dir_change_delay_us != 0) {
|
||||
if (dir_change_delay_us > MAX_DIR_DELAY_US) {
|
||||
dir_change_delay_us = MAX_DIR_DELAY_US;
|
||||
}
|
||||
if (dir_change_delay_us < MIN_DIR_DELAY_US) {
|
||||
dir_change_delay_us = MIN_DIR_DELAY_US;
|
||||
}
|
||||
_dir_change_delay_ticks = US_TO_TICKS(dir_change_delay_us);
|
||||
} else {
|
||||
_dir_change_delay_ticks = 0;
|
||||
}
|
||||
}
|
||||
void FastAccelStepper::setEnablePin(uint8_t enablePin,
|
||||
bool low_active_enables_stepper) {
|
||||
if (low_active_enables_stepper) {
|
||||
_enablePinLowActive = enablePin;
|
||||
if (enablePin != PIN_UNDEFINED) {
|
||||
if (enablePin & PIN_EXTERNAL_FLAG) {
|
||||
if (_engine->_externalCallForPin) {
|
||||
_engine->_externalCallForPin(enablePin, HIGH);
|
||||
}
|
||||
} else {
|
||||
PIN_OUTPUT(enablePin, HIGH);
|
||||
if (_enablePinHighActive == enablePin) {
|
||||
_enablePinHighActive = PIN_UNDEFINED;
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
_enablePinHighActive = enablePin;
|
||||
if (enablePin != PIN_UNDEFINED) {
|
||||
if (enablePin & PIN_EXTERNAL_FLAG) {
|
||||
if (_engine->_externalCallForPin) {
|
||||
_engine->_externalCallForPin(enablePin, LOW);
|
||||
}
|
||||
} else {
|
||||
PIN_OUTPUT(enablePin, LOW);
|
||||
if (_enablePinLowActive == enablePin) {
|
||||
_enablePinLowActive = PIN_UNDEFINED;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
void FastAccelStepper::setAutoEnable(bool auto_enable) {
|
||||
_autoEnable = auto_enable;
|
||||
if (auto_enable && (_off_delay_count == 0)) {
|
||||
_off_delay_count = 1;
|
||||
}
|
||||
}
|
||||
int8_t FastAccelStepper::setDelayToEnable(uint32_t delay_us) {
|
||||
uint32_t delay_ticks = US_TO_TICKS(delay_us);
|
||||
if (delay_ticks > 0) {
|
||||
if (delay_ticks < MIN_CMD_TICKS) {
|
||||
return DELAY_TOO_LOW;
|
||||
}
|
||||
}
|
||||
if (delay_ticks > MAX_ON_DELAY_TICKS) {
|
||||
return DELAY_TOO_HIGH;
|
||||
}
|
||||
_on_delay_ticks = delay_ticks;
|
||||
return DELAY_OK;
|
||||
}
|
||||
void FastAccelStepper::setDelayToDisable(uint16_t delay_ms) {
|
||||
uint16_t delay_count = delay_ms / DELAY_MS_BASE;
|
||||
if ((delay_ms > 0) && (delay_count < 2)) {
|
||||
// ensure minimum time
|
||||
delay_count = 2;
|
||||
}
|
||||
_off_delay_count = fas_max(delay_count, (uint16_t)1);
|
||||
}
|
||||
int8_t FastAccelStepper::runForward() { return _rg.startRun(true); }
|
||||
int8_t FastAccelStepper::runBackward() { return _rg.startRun(false); }
|
||||
int8_t FastAccelStepper::moveTo(int32_t position, bool blocking) {
|
||||
int8_t res = _rg.moveTo(position, &fas_queue[_queue_num].queue_end);
|
||||
if ((res == MOVE_OK) && blocking) {
|
||||
while (isRunning()) {
|
||||
noop_or_wait;
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
int8_t FastAccelStepper::move(int32_t move, bool blocking) {
|
||||
if ((move < 0) && (_dirPin == PIN_UNDEFINED)) {
|
||||
return MOVE_ERR_NO_DIRECTION_PIN;
|
||||
}
|
||||
int8_t res = _rg.move(move, &fas_queue[_queue_num].queue_end);
|
||||
if ((res == MOVE_OK) && blocking) {
|
||||
while (isRunning()) {
|
||||
noop_or_wait;
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
void FastAccelStepper::keepRunning() { _rg.setKeepRunning(); }
|
||||
void FastAccelStepper::stopMove() { _rg.initiateStop(); }
|
||||
void FastAccelStepper::applySpeedAcceleration() {
|
||||
_rg.applySpeedAcceleration();
|
||||
}
|
||||
int8_t FastAccelStepper::moveByAcceleration(int32_t acceleration,
|
||||
bool allow_reverse) {
|
||||
int8_t res = MOVE_OK;
|
||||
if (acceleration > 0) {
|
||||
setAcceleration(acceleration);
|
||||
res = runForward();
|
||||
} else if (acceleration < 0) {
|
||||
setAcceleration(-acceleration);
|
||||
if (allow_reverse && (_dirPin != PIN_UNDEFINED)) {
|
||||
res = runBackward();
|
||||
} else {
|
||||
applySpeedAcceleration();
|
||||
stopMove();
|
||||
}
|
||||
} else {
|
||||
uint32_t max_speed = _rg.getSpeedInTicks();
|
||||
setSpeedInTicks(getPeriodInTicksAfterCommandsCompleted());
|
||||
setAcceleration(1); // ensure increase, so the speed is kept
|
||||
applySpeedAcceleration();
|
||||
setSpeedInTicks(max_speed);
|
||||
}
|
||||
return res;
|
||||
}
|
||||
void FastAccelStepper::forceStop() {
|
||||
StepperQueue* q = &fas_queue[_queue_num];
|
||||
|
||||
// ensure no more commands are added to the queue
|
||||
q->ignore_commands = true;
|
||||
|
||||
// inform ramp generator to force stop
|
||||
_rg.forceStop();
|
||||
}
|
||||
void FastAccelStepper::forceStopAndNewPosition(int32_t new_pos) {
|
||||
StepperQueue* q = &fas_queue[_queue_num];
|
||||
|
||||
// ensure no more commands are added to the queue
|
||||
q->ignore_commands = true;
|
||||
|
||||
// stop ramp generator
|
||||
_rg.stopRamp();
|
||||
|
||||
// stop the stepper interrupt and empty the queue
|
||||
q->forceStop();
|
||||
|
||||
// set the new position. This should be safe
|
||||
q->queue_end.pos = new_pos;
|
||||
_rg.setTargetPosition(new_pos);
|
||||
}
|
||||
bool FastAccelStepper::disableOutputs() {
|
||||
if (isRunning() && _autoEnable) {
|
||||
return false;
|
||||
}
|
||||
bool disabled = true;
|
||||
if (_enablePinLowActive != PIN_UNDEFINED) {
|
||||
if (_enablePinLowActive & PIN_EXTERNAL_FLAG) {
|
||||
if (_engine->_externalCallForPin != nullptr) {
|
||||
disabled &=
|
||||
(_engine->_externalCallForPin(_enablePinLowActive, HIGH) == HIGH);
|
||||
}
|
||||
} else {
|
||||
digitalWrite(_enablePinLowActive, HIGH);
|
||||
}
|
||||
}
|
||||
if (_enablePinHighActive != PIN_UNDEFINED) {
|
||||
if (_enablePinHighActive & PIN_EXTERNAL_FLAG) {
|
||||
if (_engine->_externalCallForPin != nullptr) {
|
||||
disabled &=
|
||||
(_engine->_externalCallForPin(_enablePinHighActive, LOW) == LOW);
|
||||
}
|
||||
} else {
|
||||
digitalWrite(_enablePinHighActive, LOW);
|
||||
}
|
||||
}
|
||||
if (disabled) {
|
||||
_auto_disable_delay_counter = 0;
|
||||
}
|
||||
return disabled;
|
||||
}
|
||||
bool FastAccelStepper::enableOutputs() {
|
||||
bool enabled = true;
|
||||
if (_enablePinLowActive != PIN_UNDEFINED) {
|
||||
if (_enablePinLowActive & PIN_EXTERNAL_FLAG) {
|
||||
if (_engine->_externalCallForPin != nullptr) {
|
||||
enabled &=
|
||||
(_engine->_externalCallForPin(_enablePinLowActive, LOW) == LOW);
|
||||
}
|
||||
} else {
|
||||
digitalWrite(_enablePinLowActive, LOW);
|
||||
}
|
||||
}
|
||||
if (_enablePinHighActive != PIN_UNDEFINED) {
|
||||
if (_enablePinHighActive & PIN_EXTERNAL_FLAG) {
|
||||
if (_engine->_externalCallForPin != nullptr) {
|
||||
enabled &=
|
||||
(_engine->_externalCallForPin(_enablePinHighActive, HIGH) == HIGH);
|
||||
}
|
||||
} else {
|
||||
digitalWrite(_enablePinHighActive, HIGH);
|
||||
}
|
||||
}
|
||||
return enabled;
|
||||
}
|
||||
int32_t FastAccelStepper::getPositionAfterCommandsCompleted() const {
|
||||
return fas_queue[_queue_num].queue_end.pos;
|
||||
}
|
||||
uint32_t FastAccelStepper::getPeriodInTicksAfterCommandsCompleted() {
|
||||
if (_rg.isRampGeneratorActive()) {
|
||||
return _rg.getCurrentPeriodInTicks();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
uint32_t FastAccelStepper::getPeriodInUsAfterCommandsCompleted() {
|
||||
if (_rg.isRampGeneratorActive()) {
|
||||
return _rg.getCurrentPeriodInUs();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
void FastAccelStepper::getCurrentSpeedInTicks(struct actual_ticks_s* speed,
|
||||
bool realtime) {
|
||||
bool valid;
|
||||
if (realtime) {
|
||||
valid = fas_queue[_queue_num].getActualTicksWithDirection(speed);
|
||||
} else {
|
||||
valid = false;
|
||||
}
|
||||
if (!valid) {
|
||||
if (_rg.isRampGeneratorActive()) {
|
||||
_rg.getCurrentSpeedInTicks(speed);
|
||||
} else {
|
||||
speed->ticks = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
int32_t FastAccelStepper::getCurrentSpeedInUs(bool realtime) {
|
||||
struct actual_ticks_s speed;
|
||||
getCurrentSpeedInTicks(&speed, realtime);
|
||||
int32_t speed_in_us = speed.ticks / (TICKS_PER_S / 1000000);
|
||||
if (speed.count_up) {
|
||||
return speed_in_us;
|
||||
}
|
||||
return -speed_in_us;
|
||||
}
|
||||
int32_t FastAccelStepper::getCurrentSpeedInMilliHz(bool realtime) {
|
||||
struct actual_ticks_s speed;
|
||||
getCurrentSpeedInTicks(&speed, realtime);
|
||||
if (speed.ticks > 0) {
|
||||
int32_t speed_in_mhz = ((uint32_t)250 * TICKS_PER_S) / speed.ticks * 4;
|
||||
if (speed.count_up) {
|
||||
return speed_in_mhz;
|
||||
}
|
||||
return -speed_in_mhz;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
uint16_t FastAccelStepper::getMaxSpeedInTicks() const {
|
||||
return fas_queue[_queue_num].getMaxSpeedInTicks();
|
||||
}
|
||||
uint16_t FastAccelStepper::getMaxSpeedInUs() const {
|
||||
uint16_t ticks = getMaxSpeedInTicks();
|
||||
uint16_t speed_in_us = ticks / (TICKS_PER_S / 1000000);
|
||||
return speed_in_us;
|
||||
}
|
||||
uint32_t FastAccelStepper::getMaxSpeedInHz() const {
|
||||
uint16_t ticks = getMaxSpeedInTicks();
|
||||
uint32_t speed_in_hz = TICKS_PER_S / ticks;
|
||||
return speed_in_hz;
|
||||
}
|
||||
uint32_t FastAccelStepper::getMaxSpeedInMilliHz() const {
|
||||
uint16_t ticks = getMaxSpeedInTicks();
|
||||
uint32_t speed_in_milli_hz = ((uint32_t)250 * TICKS_PER_S) / ticks * 4;
|
||||
return speed_in_milli_hz;
|
||||
}
|
||||
#if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING == 1
|
||||
void FastAccelStepper::setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks) const {
|
||||
fas_queue[_queue_num].setAbsoluteSpeedLimit(max_speed_in_ticks);
|
||||
}
|
||||
#endif
|
||||
int8_t FastAccelStepper::setSpeedInTicks(uint32_t min_step_ticks) {
|
||||
if (min_step_ticks < getMaxSpeedInTicks()) {
|
||||
return -1;
|
||||
}
|
||||
if (min_step_ticks == TICKS_FOR_STOPPED_MOTOR) {
|
||||
return -1;
|
||||
}
|
||||
_rg.setSpeedInTicks(min_step_ticks);
|
||||
return 0;
|
||||
}
|
||||
int8_t FastAccelStepper::setSpeedInUs(uint32_t min_step_us) {
|
||||
if (min_step_us >= TICKS_TO_US(0xffffffff)) {
|
||||
return -1;
|
||||
}
|
||||
uint32_t min_step_ticks = US_TO_TICKS(min_step_us);
|
||||
return setSpeedInTicks(min_step_ticks);
|
||||
}
|
||||
int8_t FastAccelStepper::setSpeedInHz(uint32_t speed_hz) {
|
||||
if (speed_hz == 0) {
|
||||
return -1;
|
||||
}
|
||||
uint32_t ticks = _rg.divForHz(speed_hz);
|
||||
return setSpeedInTicks(ticks);
|
||||
}
|
||||
int8_t FastAccelStepper::setSpeedInMilliHz(uint32_t speed_mhz) {
|
||||
if (speed_mhz <= (1000LL * TICKS_PER_S / 0xffffffff + 1)) {
|
||||
return -1;
|
||||
}
|
||||
uint32_t ticks = _rg.divForMilliHz(speed_mhz);
|
||||
return setSpeedInTicks(ticks);
|
||||
}
|
||||
void FastAccelStepper::setCurrentPosition(int32_t new_pos) {
|
||||
int32_t delta = new_pos - getCurrentPosition();
|
||||
if (delta != 0) {
|
||||
struct queue_end_s* queue_end = &fas_queue[_queue_num].queue_end;
|
||||
fasDisableInterrupts();
|
||||
queue_end->pos = queue_end->pos + delta;
|
||||
_rg.advanceTargetPosition(delta, queue_end);
|
||||
fasEnableInterrupts();
|
||||
}
|
||||
}
|
||||
void FastAccelStepper::setPositionAfterCommandsCompleted(int32_t new_pos) {
|
||||
struct queue_end_s* queue_end = &fas_queue[_queue_num].queue_end;
|
||||
fasDisableInterrupts();
|
||||
int32_t delta = new_pos - fas_queue[_queue_num].queue_end.pos;
|
||||
queue_end->pos = new_pos;
|
||||
if (delta != 0) {
|
||||
_rg.advanceTargetPosition(delta, queue_end);
|
||||
}
|
||||
fasEnableInterrupts();
|
||||
}
|
||||
uint8_t FastAccelStepper::queueEntries() const {
|
||||
return fas_queue[_queue_num].queueEntries();
|
||||
}
|
||||
uint32_t FastAccelStepper::ticksInQueue() {
|
||||
return fas_queue[_queue_num].ticksInQueue();
|
||||
}
|
||||
bool FastAccelStepper::hasTicksInQueue(uint32_t min_ticks) const {
|
||||
return fas_queue[_queue_num].hasTicksInQueue(min_ticks);
|
||||
}
|
||||
bool FastAccelStepper::isQueueFull() const {
|
||||
return fas_queue[_queue_num].isQueueFull();
|
||||
}
|
||||
bool FastAccelStepper::isQueueEmpty() const {
|
||||
return fas_queue[_queue_num].isQueueEmpty();
|
||||
}
|
||||
bool FastAccelStepper::isQueueRunning() const {
|
||||
return fas_queue[_queue_num].isRunning();
|
||||
}
|
||||
bool FastAccelStepper::isRunning() {
|
||||
StepperQueue* q = &fas_queue[_queue_num];
|
||||
return q->isRunning() || _rg.isRampGeneratorActive() || !isQueueEmpty();
|
||||
}
|
||||
void FastAccelStepper::performOneStep(bool count_up, bool blocking) {
|
||||
if (!isRunning()) {
|
||||
if (count_up || (_dirPin != PIN_UNDEFINED)) {
|
||||
struct stepper_command_s cmd = {
|
||||
.ticks = MIN_CMD_TICKS, .steps = 1, .count_up = count_up};
|
||||
addQueueEntry(&cmd);
|
||||
if (blocking) {
|
||||
while (isRunning()) {
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
void FastAccelStepper::forwardStep(bool blocking) {
|
||||
performOneStep(true, blocking);
|
||||
}
|
||||
void FastAccelStepper::backwardStep(bool blocking) {
|
||||
performOneStep(false, blocking);
|
||||
}
|
||||
int32_t FastAccelStepper::getCurrentPosition() const {
|
||||
return fas_queue[_queue_num].getCurrentPosition();
|
||||
}
|
||||
void FastAccelStepper::detachFromPin() const { fas_queue[_queue_num].disconnect(); }
|
||||
void FastAccelStepper::reAttachToPin() const { fas_queue[_queue_num].connect(); }
|
||||
770
src/FastAccelStepper.h
Normal file
770
src/FastAccelStepper.h
Normal file
@@ -0,0 +1,770 @@
|
||||
#ifndef FASTACCELSTEPPER_H
|
||||
#define FASTACCELSTEPPER_H
|
||||
#include <stdint.h>
|
||||
#include "PoorManFloat.h"
|
||||
#include "fas_arch/common.h"
|
||||
|
||||
// # FastAccelStepper
|
||||
//
|
||||
// FastAccelStepper is a high speed alternative for the
|
||||
// [AccelStepper library](http://www.airspayce.com/mikem/arduino/AccelStepper/).
|
||||
// Supported are avr (ATmega 168/328/P, ATmega2560), esp32 and atmelsam due.
|
||||
//
|
||||
// Here is a basic example to run a stepper from position 0 to 1000 and back
|
||||
// again to 0.
|
||||
// ```
|
||||
// #include <FastAccelStepper.h>
|
||||
//
|
||||
// FastAccelStepperEngine engine = FastAccelStepperEngine();
|
||||
// FastAccelStepper *stepper = NULL;
|
||||
//
|
||||
// #define dirPinStepper 5
|
||||
// #define enablePinStepper 6
|
||||
// #define stepPinStepper 9
|
||||
// void setup() {
|
||||
// engine.init();
|
||||
// stepper = engine.stepperConnectToPin(stepPinStepper);
|
||||
// if (stepper) {
|
||||
// stepper->setDirectionPin(dirPinStepper);
|
||||
// stepper->setEnablePin(enablePinStepper);
|
||||
// stepper->setAutoEnable(true);
|
||||
//
|
||||
// stepper->setSpeedInHz(500);
|
||||
// stepper->setAcceleration(100);
|
||||
// stepper->moveTo(1000, true);
|
||||
// stepper->moveTo(0, true);
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// void loop() {}
|
||||
// ```
|
||||
|
||||
class FastAccelStepper;
|
||||
|
||||
class FastAccelStepperEngine {
|
||||
//
|
||||
// ## FastAccelStepperEngine
|
||||
//
|
||||
// This engine - actually a factory - provides you with instances of steppers.
|
||||
|
||||
public:
|
||||
// ### Initialization
|
||||
//
|
||||
// The FastAccelStepperEngine is declared with FastAccelStepperEngine().
|
||||
// This is to occupy the needed memory.
|
||||
// ```cpp
|
||||
// FastAccelStepperEngine engine = FastAccelStepperEngine();
|
||||
// ```
|
||||
// But it still needs to be initialized.
|
||||
// For this init shall be used:
|
||||
// ```cpp
|
||||
// void setup() {
|
||||
// engine.init();
|
||||
// }
|
||||
// ```
|
||||
|
||||
void init();
|
||||
|
||||
#if defined(SUPPORT_CPU_AFFINITY)
|
||||
// In a multitasking and multicore system like ESP32, the steppers are
|
||||
// controlled by a continuously running task. This task can be fixed to one
|
||||
// CPU core with this modified init()-call. ESP32 implementation detail: For
|
||||
// values 0 and 1, xTaskCreatePinnedToCore() is used, or else xTaskCreate()
|
||||
void init(uint8_t cpu_core);
|
||||
#endif
|
||||
|
||||
// ### Creation of FastAccelStepper
|
||||
//
|
||||
// Using a call to `stepperConnectToPin()` a FastAccelStepper instance is
|
||||
// created. This call tells the stepper, which step pin to use. As the
|
||||
// hardware may have limitations - e.g. no stepper resources anymore, or the
|
||||
// step pin cannot be used, then NULL is returned. So it is advised to check
|
||||
// the return value of this call.
|
||||
#if !defined(SUPPORT_SELECT_DRIVER_TYPE)
|
||||
FastAccelStepper* stepperConnectToPin(uint8_t step_pin);
|
||||
#endif
|
||||
// For e.g. esp32, there are two types of driver.
|
||||
// One using mcpwm and pcnt module. And another using rmt module.
|
||||
// This call allows to select the respective driver
|
||||
#if defined(SUPPORT_SELECT_DRIVER_TYPE)
|
||||
#define DRIVER_MCPWM_PCNT 0
|
||||
#define DRIVER_RMT 1
|
||||
#define DRIVER_DONT_CARE 2
|
||||
FastAccelStepper* stepperConnectToPin(uint8_t step_pin,
|
||||
uint8_t driver_type = DRIVER_DONT_CARE);
|
||||
#endif
|
||||
|
||||
// Comments to valid pins:
|
||||
//
|
||||
// clang-format off
|
||||
// | Device | Comment |
|
||||
// |:----------------|:--------------------------------------------------------------------------------------------------|
|
||||
// | ESP32 | Every output capable GPIO can be used |
|
||||
// | ESP32S2 | Every output capable GPIO can be used |
|
||||
// | Atmega168/328/p | Only the pins connected to OC1A and OC1B are allowed |
|
||||
// | Atmega2560 | Only the pins connected to OC4A, OC4B and OC4C are allowed. |
|
||||
// | Atmega32u4 | Only the pins connected to OC1A, OC1B and OC1C are allowed |
|
||||
// | Atmel SAM | This can be one of each group of pins: 34/67/74/35, 17/36/72/37/42, 40/64/69/41, 9, 8/44, 7/45, 6 |
|
||||
// clang-format on
|
||||
|
||||
// ## External Pins
|
||||
//
|
||||
// If the direction/enable pins are e.g. connected via external HW (shift
|
||||
// registers), then an external callback function can be supplied. The
|
||||
// supplied value is either LOW or HIGH. The return value shall be the status
|
||||
// of the pin (false for LOW or true for HIGH). If returned value and supplied
|
||||
// value do not match, the stepper does not continue, but calls this function
|
||||
// again.
|
||||
//
|
||||
// This function is called from cyclic task/interrupt with 4ms rate, which
|
||||
// creates the commands to put into the command queue. Thus the supplied
|
||||
// function should take much less time than 4ms. Otherwise there is risk, that
|
||||
// other running steppers are running out of commands in the queue. If this
|
||||
// takes longer, then the function should be offloaded and return the new
|
||||
// status, after the pin change has been successfully completed.
|
||||
//
|
||||
// The callback has to be called on the FastAccelStepperEngine.
|
||||
// See examples/ExternalCall
|
||||
//
|
||||
// Stepperpins (enable or direction), which should use this external callback,
|
||||
// need to be or'ed with PIN_EXTERNAL_FLAG ! FastAccelStepper uses this flag
|
||||
// to determine, if a pin is external or internal.
|
||||
void setExternalCallForPin(bool (*func)(uint8_t pin, uint8_t value));
|
||||
|
||||
// ### Debug LED
|
||||
//
|
||||
// If blinking of a LED is required to indicate, the stepper controller is
|
||||
// still running, then the port. to which the LED is connected, can be told to
|
||||
// the engine. The periodic task will let the associated LED blink with 1 Hz
|
||||
static void setDebugLed(uint8_t ledPin);
|
||||
|
||||
/* This should be only called from ISR or stepper task. So do not call it */
|
||||
void manageSteppers();
|
||||
|
||||
private:
|
||||
bool isDirPinBusy(uint8_t dirPin, uint8_t except_stepper);
|
||||
|
||||
uint8_t _stepper_cnt;
|
||||
FastAccelStepper* _stepper[MAX_STEPPER];
|
||||
|
||||
static bool _isValidStepPin(uint8_t step_pin);
|
||||
bool (*_externalCallForPin)(uint8_t pin, uint8_t value);
|
||||
|
||||
friend class FastAccelStepper;
|
||||
};
|
||||
|
||||
// ### Return codes of calls to `move()` and `moveTo()`
|
||||
//
|
||||
// The defined preprocessor macros are MOVE_xxx:
|
||||
// MOVE_OK: All is OK:
|
||||
// MOVE_ERR_NO_DIRECTION_PIN: Negative direction requested, but no direction pin
|
||||
// MOVE_ERR_SPEED_IS_UNDEFINED: The maximum speed has not been set yet
|
||||
// MOVE_ERR_ACCELERATION_IS_UNDEFINED: The acceleration to use has not been set
|
||||
// yet
|
||||
|
||||
// ### Return codes of `rampState()`
|
||||
//
|
||||
// The return value is an uint8_t, which consist of two fields:
|
||||
//
|
||||
// | Bit 7 | Bits 6-5 | Bits 4-0 |
|
||||
// |:--------|:----------|:---------|
|
||||
// |Always 0 | Direction | State |
|
||||
//
|
||||
// The bit mask for direction and state:
|
||||
#define RAMP_DIRECTION_MASK (32 + 64)
|
||||
#define RAMP_STATE_MASK (1 + 2 + 4 + 8 + 16)
|
||||
|
||||
// The defined ramp states are:
|
||||
#define RAMP_STATE_IDLE 0
|
||||
#define RAMP_STATE_COAST 1
|
||||
#define RAMP_STATE_ACCELERATE 2
|
||||
#define RAMP_STATE_DECELERATE 4
|
||||
#define RAMP_STATE_REVERSE (4 + 8)
|
||||
#define RAMP_STATE_ACCELERATING_FLAG 2
|
||||
#define RAMP_STATE_DECELERATING_FLAG 4
|
||||
|
||||
// And the two directions of a move
|
||||
#define RAMP_DIRECTION_COUNT_UP 32
|
||||
#define RAMP_DIRECTION_COUNT_DOWN 64
|
||||
|
||||
// A ramp state value of 2 is set after any move call on a stopped motor
|
||||
// and until the stepper task is serviced. The stepper task will then
|
||||
// control the direction flags
|
||||
|
||||
#include "RampGenerator.h"
|
||||
|
||||
//
|
||||
// ## Timing values - Architecture dependent
|
||||
//
|
||||
// ### AVR
|
||||
// |VARIABLE | Value | Unit |
|
||||
// |:----------------|------------:|:------------------------|
|
||||
// |TICKS_PER_S | 16_000_000 | [ticks/s] |
|
||||
// |MIN_CMD_TICKS | 640 | [1/TICKS_PER_S seconds] |
|
||||
// |MIN_DIR_DELAY_US | 40 | [µs] |
|
||||
// |MAX_DIR_DELAY_US | 4095 | [µs] |
|
||||
//
|
||||
// ### ESP32
|
||||
// |VARIABLE | Value | Unit |
|
||||
// |:----------------|------------:|:------------------------|
|
||||
// |TICKS_PER_S | 16_000_000 | [ticks/s] |
|
||||
// |MIN_CMD_TICKS | 3200 | [1/TICKS_PER_S seconds] |
|
||||
// |MIN_DIR_DELAY_US | 200 | [µs] |
|
||||
// |MAX_DIR_DELAY_US | 4095 | [µs] |
|
||||
//
|
||||
// ### SAM DUE
|
||||
// |VARIABLE | Value | Unit |
|
||||
// |:----------------|------------:|:------------------------|
|
||||
// |TICKS_PER_S | 21_000_000 | [ticks/s] |
|
||||
// |MIN_CMD_TICKS | 4200 | [1/TICKS_PER_S seconds] |
|
||||
// |MIN_DIR_DELAY_US | 200 | [µs] |
|
||||
// |MAX_DIR_DELAY_US | 3120 | [µs] |
|
||||
//
|
||||
// # FastAccelStepper
|
||||
|
||||
#define MAX_ON_DELAY_TICKS ((uint32_t)(65535 * (QUEUE_LEN - 1)))
|
||||
|
||||
#define PIN_UNDEFINED 255
|
||||
#define PIN_EXTERNAL_FLAG 128
|
||||
|
||||
class FastAccelStepper {
|
||||
#ifdef TEST
|
||||
public:
|
||||
#else
|
||||
private:
|
||||
#endif
|
||||
void init(FastAccelStepperEngine* engine, uint8_t num, uint8_t step_pin);
|
||||
|
||||
public:
|
||||
// ## Step Pin
|
||||
// step pin is defined at creation. Here can retrieve the pin
|
||||
uint8_t getStepPin() const;
|
||||
|
||||
// ## Direction Pin
|
||||
// if direction pin is connected, call this function.
|
||||
//
|
||||
// If the pin number is >= 128, then the direction pin is assumed to be
|
||||
// external and the external callback function (set by
|
||||
// `setExternalCallForPin()`) is used to set the pin. For direction pin, this
|
||||
// is implemented for esp32 and its supported derivates, and avr and its
|
||||
// derivates except atmega32u4
|
||||
//
|
||||
// For slow driver hardware the first step after any polarity change of the
|
||||
// direction pin can be delayed by the value dir_change_delay_us. The allowed
|
||||
// range is MIN_DIR_DELAY_US and MAX_DIR_DELAY_US. The special value of 0
|
||||
// means, that no delay is added. Values 1 up to MIN_DIR_DELAY_US will be
|
||||
// clamped to MIN_DIR_DELAY_US. Values above MAX_DIR_DELAY_US will be clamped
|
||||
// to MAX_DIR_DELAY_US. For external pins, dir_change_delay_us is ignored,
|
||||
// because the mechanism applied for external pins provides already pause
|
||||
// in the range of ms or more.
|
||||
void setDirectionPin(uint8_t dirPin, bool dirHighCountsUp = true,
|
||||
uint16_t dir_change_delay_us = 0);
|
||||
inline uint8_t getDirectionPin() { return _dirPin; }
|
||||
inline bool directionPinHighCountsUp() { return _dirHighCountsUp; }
|
||||
|
||||
// ## Enable Pin
|
||||
// if enable pin is connected, then use this function.
|
||||
//
|
||||
// If the pin number is >= 128, then the enable pin is assumed to be
|
||||
// external and the external callback function (set by
|
||||
// `setExternalCallForPin()`) is used to set the pin.
|
||||
//
|
||||
// In case there are two enable pins: one low and one high active, then
|
||||
// these calls are valid and both pins will be operated:
|
||||
// setEnablePin(pin1, true);
|
||||
// setEnablePin(pin2, false);
|
||||
// If pin1 and pin2 are same, then the last call will be used.
|
||||
void setEnablePin(uint8_t enablePin, bool low_active_enables_stepper = true);
|
||||
inline uint8_t getEnablePinHighActive() { return _enablePinHighActive; }
|
||||
inline uint8_t getEnablePinLowActive() { return _enablePinLowActive; }
|
||||
|
||||
// using enableOutputs/disableOutputs the stepper can be enabled and disabled
|
||||
// For a running motor with autoEnable set, disableOutputs() will return false
|
||||
bool enableOutputs(); // returns true, if enabled
|
||||
bool disableOutputs(); // returns true, if disabled
|
||||
|
||||
// In auto enable mode, the stepper is enabled before stepping and disabled
|
||||
// afterwards. The delay from stepper enabled till first step and from
|
||||
// last step to stepper disabled can be separately adjusted.
|
||||
// The delay from enable to first step is done in ticks and as such is limited
|
||||
// to MAX_ON_DELAY_TICKS, which translates approximately to 120ms for
|
||||
// esp32 and 60ms for avr at 16 MHz). The delay till disable is done in period
|
||||
// interrupt/task with 4 or 10 ms repetition rate and as such is with several
|
||||
// ms jitter.
|
||||
void setAutoEnable(bool auto_enable);
|
||||
int8_t setDelayToEnable(uint32_t delay_us);
|
||||
void setDelayToDisable(uint16_t delay_ms);
|
||||
#define DELAY_OK 0
|
||||
#define DELAY_TOO_LOW -1
|
||||
#define DELAY_TOO_HIGH -2
|
||||
|
||||
// ## Stepper Position
|
||||
// Retrieve the current position of the stepper
|
||||
//
|
||||
// Comment for esp32 with rmt module:
|
||||
// The actual position may be off by the number of steps in the ongoing
|
||||
// command. If precise real time position is needed, attaching a pulse counter
|
||||
// may be of help.
|
||||
int32_t getCurrentPosition() const;
|
||||
|
||||
// Set the current position of the stepper - either in standstill or while
|
||||
// moving.
|
||||
// for esp32: the implementation uses getCurrentPosition(), which does not
|
||||
// consider the steps of the current command
|
||||
// => recommend to use only in standstill
|
||||
void setCurrentPosition(int32_t new_pos);
|
||||
|
||||
// ## Stepper running status
|
||||
// is true while the stepper is running or ramp generation is active
|
||||
bool isRunning();
|
||||
|
||||
// ## Speed
|
||||
// For stepper movement control by FastAccelStepper's ramp generator
|
||||
//
|
||||
// Speed can be defined in four different units:
|
||||
// - In Hz: This means steps/s
|
||||
// - In millHz: This means in steps/1000s
|
||||
// - In us: This means in us/step
|
||||
//
|
||||
// For the device's maximum allowed speed, the following calls can be used.
|
||||
uint16_t getMaxSpeedInUs() const;
|
||||
uint16_t getMaxSpeedInTicks() const;
|
||||
uint32_t getMaxSpeedInHz() const;
|
||||
uint32_t getMaxSpeedInMilliHz() const;
|
||||
|
||||
// For esp32 and avr, the device's maximum allowed speed can be overridden.
|
||||
// Allocating a new stepper will override any absolute speed limit.
|
||||
// This is absolutely untested, no error checking implemented.
|
||||
// Use at your own risk !
|
||||
#if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING == 1
|
||||
void setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks) const;
|
||||
#endif
|
||||
|
||||
// Setting the speed can be done with the four `setSpeed...()` calls.
|
||||
// The new value will be used only after call of these functions:
|
||||
//
|
||||
// - `move()`
|
||||
// - `moveTo()`
|
||||
// - `runForward()`
|
||||
// - `runBackward()`
|
||||
// - `applySpeedAcceleration()`
|
||||
// - `moveByAcceleration()`
|
||||
//
|
||||
// Note: no update on `stopMove()`
|
||||
//
|
||||
// Returns 0 on success, or -1 on invalid value.
|
||||
// Invalid is faster than MaxSpeed or slower than ~250 Mio ticks/step.
|
||||
int8_t setSpeedInUs(uint32_t min_step_us);
|
||||
int8_t setSpeedInTicks(uint32_t min_step_ticks);
|
||||
int8_t setSpeedInHz(uint32_t speed_hz);
|
||||
int8_t setSpeedInMilliHz(uint32_t speed_mhz);
|
||||
|
||||
// To retrieve current set speed. This means, while accelerating and/or
|
||||
// decelerating, this is NOT the actual speed !
|
||||
inline uint32_t getSpeedInUs() { return _rg.getSpeedInUs(); }
|
||||
inline uint32_t getSpeedInTicks() { return _rg.getSpeedInTicks(); }
|
||||
inline uint32_t getSpeedInMilliHz() { return _rg.getSpeedInMilliHz(); }
|
||||
|
||||
// If the current speed is needed, then use `getCurrentSpeed...()`. This
|
||||
// retrieves the actual speed.
|
||||
//
|
||||
// | value | description |
|
||||
// |:-----:|:-----------------------------|
|
||||
// | = 0 | while not moving |
|
||||
// | > 0 | while position counting up |
|
||||
// | < 0 | while position counting down |
|
||||
//
|
||||
// If the parameter realtime is true, then the most actual speed
|
||||
// from the stepper queue is derived. This works only, if the queue
|
||||
// does not contain pauses, which is normally the case for slow speeds.
|
||||
// Otherwise the speed from the ramp generator is reported, which is
|
||||
// done always in case of `realtime == false`. That speed is typically
|
||||
// the value of the speed a couple of milliseconds later.
|
||||
//
|
||||
// The drawback of `realtime == true` is, that the reported speed
|
||||
// may either come from the queue or from the ramp generator.
|
||||
// This means the returned speed may have jumps during
|
||||
// acceleration/deceleration.
|
||||
//
|
||||
// For backward compatibility, the default is true.
|
||||
int32_t getCurrentSpeedInUs(bool realtime = true);
|
||||
int32_t getCurrentSpeedInMilliHz(bool realtime = true);
|
||||
|
||||
// ## Acceleration
|
||||
// setAcceleration() expects as parameter the change of speed
|
||||
// as step/s².
|
||||
// If for example the speed should ramp up from 0 to 10000 steps/s within
|
||||
// 10s, then the acceleration is 10000 steps/s / 10s = 1000 steps/s²
|
||||
//
|
||||
// New value will be used after call to
|
||||
// move/moveTo/runForward/runBackward/applySpeedAcceleration/moveByAcceleration
|
||||
//
|
||||
// note: no update on stopMove()
|
||||
//
|
||||
// Returns 0 on success, or -1 on invalid value (<=0)
|
||||
inline int8_t setAcceleration(int32_t step_s_s) {
|
||||
return _rg.setAcceleration(step_s_s);
|
||||
}
|
||||
inline uint32_t getAcceleration() { return _rg.getAcceleration(); }
|
||||
|
||||
// getCurrentAcceleration() retrieves the actual acceleration.
|
||||
// = 0 while idle or coasting
|
||||
// > 0 while speed is changing towards positive values
|
||||
// < 0 while speed is changeing towards negative values
|
||||
inline int32_t getCurrentAcceleration() {
|
||||
return _rg.getCurrentAcceleration();
|
||||
}
|
||||
|
||||
// ## Linear Acceleration
|
||||
// setLinearAcceleration expects as parameter the number of steps,
|
||||
// where the acceleration is increased linearly from standstill up to the
|
||||
// configured acceleration value. If this parameter is 0, then there will be
|
||||
// no linear acceleration phase
|
||||
//
|
||||
// If for example the acceleration should ramp up from 0 to 10000 steps/s^2
|
||||
// within 100 steps, then call setLinearAcceleration(100)
|
||||
//
|
||||
// The speed at which linear acceleration turns into constant acceleration
|
||||
// can be calculated from the parameter linear_acceleration_steps.
|
||||
// Let's call this parameter `s_h` for handover steps.
|
||||
// Then the speed is:
|
||||
// `v_h = sqrt(1.5 * a * s_h)`
|
||||
//
|
||||
// New value will be used after call to
|
||||
// move/moveTo/runForward/runBackward/applySpeedAcceleration/moveByAcceleration
|
||||
//
|
||||
// note: no update on stopMove()
|
||||
inline void setLinearAcceleration(uint32_t linear_acceleration_steps) {
|
||||
_rg.setLinearAcceleration(linear_acceleration_steps);
|
||||
}
|
||||
|
||||
// ## Jump Start
|
||||
// setJumpStart expects as parameter the ramp step to start from standstill.
|
||||
//
|
||||
// The speed at which the stepper will start can be calculated like this:
|
||||
// - If linear acceleration is not in use:
|
||||
// start speed `v = sqrt(2 * a * jump_step)`
|
||||
// - If linear acceleration is in use and `jump_step <= s_h`:
|
||||
// start speed `v = sqrt(1.5*a)/s_h^(1/6) * jump_step^(2/3)`
|
||||
// - If linear acceleration is in use and `jump_step > s_h`:
|
||||
// start speed `v = sqrt(2 * a * (jump_step - s_h/4))`
|
||||
//
|
||||
//
|
||||
// New value will be used after call to
|
||||
// move/moveTo/runForward/runBackward
|
||||
inline void setJumpStart(uint32_t jump_step) { _rg.setJumpStart(jump_step); }
|
||||
|
||||
// ## Apply new speed/acceleration value
|
||||
// This function applies new values for speed/acceleration.
|
||||
// This is convenient especially, if the stepper is set to continuous running.
|
||||
void applySpeedAcceleration();
|
||||
|
||||
// ## Move commands
|
||||
// ### move() and moveTo()
|
||||
// start/move the stepper for (move) steps or to an absolute position.
|
||||
//
|
||||
// If the stepper is already running, then the current running move will be
|
||||
// updated together with any updated values of acceleration/speed. The move is
|
||||
// relative to the target position of any ongoing move ! If the new
|
||||
// move/moveTo for an ongoing command would reverse the direction, then the
|
||||
// command is silently ignored.
|
||||
// return values are the MOVE_... constants
|
||||
int8_t move(int32_t move, bool blocking = false);
|
||||
int8_t moveTo(int32_t position, bool blocking = false);
|
||||
|
||||
// ### keepRunning()
|
||||
// This command flags the stepper to keep run continuously into current
|
||||
// direction. It can be stopped by stopMove.
|
||||
// Be aware, if the motor is currently decelerating towards reversed
|
||||
// direction, then keepRunning() will speed up again and not finish direction
|
||||
// reversal first.
|
||||
void keepRunning();
|
||||
bool isRunningContinuously() { return _rg.isRunningContinuously(); }
|
||||
|
||||
// ### runForward() and runBackwards()
|
||||
// These commands just let the motor run continuously in one direction.
|
||||
// If the motor is running in the opposite direction, it will reverse
|
||||
// return value as with move/moveTo
|
||||
int8_t runForward();
|
||||
int8_t runBackward();
|
||||
|
||||
// ### forwardStep() and backwardStep()
|
||||
// forwardStep()/backwardstep() can be called, while stepper is not moving
|
||||
// If stepper is moving, this is a no-op.
|
||||
// backwardStep() is a no-op, if no direction pin defined
|
||||
// It will immediately let the stepper perform one single step.
|
||||
// If blocking = true, then the routine will wait till isRunning() is false
|
||||
void forwardStep(bool blocking = false);
|
||||
void backwardStep(bool blocking = false);
|
||||
|
||||
// ### moveByAcceleration()
|
||||
// moveByAcceleration() can be called, if only the speed of the stepper
|
||||
// is of interest and that speed to be controlled by acceleration.
|
||||
// The maximum speed (in both directions) to be set by setSpeedInUs() before.
|
||||
// The behaviour will be:
|
||||
// acceleration > 0 => accelerate towards positive maximum speed
|
||||
// acceleration = 0 => keep current speed
|
||||
// acceleration < 0
|
||||
// => accelerate towards negative maximum speed if allow_reverse
|
||||
// => decelerate towards motor stop if allow_reverse = false
|
||||
// return value as with move/moveTo
|
||||
int8_t moveByAcceleration(int32_t acceleration, bool allow_reverse = true);
|
||||
|
||||
// ### stopMove()
|
||||
// Stop the running stepper with normal deceleration.
|
||||
// This only sets a flag and can be called from an interrupt !
|
||||
void stopMove();
|
||||
inline bool isStopping() { return _rg.isStopping(); }
|
||||
|
||||
// ### stepsToStop()
|
||||
// This returns the current step value of the ramp.
|
||||
// This equals the number of steps for a motor to
|
||||
// reach the current position and speed from standstill
|
||||
// and to come to standstill with deceleration if stopped
|
||||
// immediately.
|
||||
// This value is valid with or without linear acceleration
|
||||
// being used.
|
||||
// Primary use is to forecast possible stop position.
|
||||
// The stop position is:
|
||||
// getCurrentPosition() + stepsToStop()
|
||||
// in case of a motor running in positive direction.
|
||||
uint32_t stepsToStop() { return _rg.stepsToStop(); }
|
||||
|
||||
// ### forceStop()
|
||||
// Abruptly stop the running stepper without deceleration.
|
||||
// This can be called from an interrupt !
|
||||
//
|
||||
// The stepper command queue will be processed, but no further commands are
|
||||
// added. This means, that the stepper can be expected to stop within approx.
|
||||
// 20ms.
|
||||
void forceStop();
|
||||
|
||||
// abruptly stop the running stepper without deceleration.
|
||||
// This can be called from an interrupt !
|
||||
//
|
||||
// No further step will be issued. As this is aborting all commands in the
|
||||
// queue, the actual stop position is lost (recovering this position cannot be
|
||||
// done within an interrupt). So the new position after stop has to be
|
||||
// provided and will be set as current position after stop.
|
||||
void forceStopAndNewPosition(int32_t new_pos);
|
||||
|
||||
// get the target position for the current move.
|
||||
// As of now, this position is the view of the stepper task.
|
||||
// This means, the value will stay unchanged after a move/moveTo until the
|
||||
// stepper task is executed.
|
||||
// In keep running mode, the targetPos() is not updated
|
||||
inline int32_t targetPos() { return _rg.targetPosition(); }
|
||||
|
||||
// ### Task planning
|
||||
// The stepper task adds commands to the stepper queue until
|
||||
// either at least two commands are planned, or the commands
|
||||
// cover sufficient time into the future. Default value for that time is 20ms.
|
||||
//
|
||||
// The stepper task is cyclically executed every ~4ms.
|
||||
// Especially for avr, the step interrupts puts a significant load on the uC,
|
||||
// so the cyclical stepper task can even run for 2-3 ms. On top of that,
|
||||
// other interrupts caused by the application could increase the load even
|
||||
// further.
|
||||
//
|
||||
// Consequently, the forward planning should fill the queue for ideally two
|
||||
// cycles, this means 8ms. This means, the default 20ms provide a sufficient
|
||||
// margin and even a missed cycle is not an issue.
|
||||
//
|
||||
// The drawback of the 20ms is, that any change in speed/acceleration are
|
||||
// added after those 20ms and for an application, requiring fast reaction
|
||||
// times, this may impact the expected performance.
|
||||
//
|
||||
// Due to this the forward planning time can be adjusted with the following
|
||||
// API call for each stepper individually.
|
||||
//
|
||||
// Attention:
|
||||
// - This is only for advanced users: no error checking is implemented.
|
||||
// - Only change the forward planning time, if the stepper is not running.
|
||||
// - Too small values bear the risk of a stepper running at full speed
|
||||
// suddenly stopping
|
||||
// due to lack of commands in the queue.
|
||||
inline void setForwardPlanningTimeInMs(uint8_t ms) {
|
||||
_forward_planning_in_ticks = ms;
|
||||
_forward_planning_in_ticks *= TICKS_PER_S / 1000; // ticks per ms
|
||||
}
|
||||
|
||||
// ## Low Level Stepper Queue Management (low level access)
|
||||
//
|
||||
// If the queue is already running, then the start parameter is obsolote.
|
||||
// But the queue may run out of commands while executing addQueueEntry,
|
||||
// so it is better to set start=true to automatically restart/continue
|
||||
// a running queue.
|
||||
//
|
||||
// If the queue is not running, then the start parameter defines starting it
|
||||
// or not. The latter case is of interest to first fill the queue and then
|
||||
// start it.
|
||||
//
|
||||
// The call addQueueEntry(NULL, true) just starts the queue. This is intended
|
||||
// to achieve a near synchronous start of several steppers. Consequently it
|
||||
// should be called with interrupts disabled and return very fast.
|
||||
// Actually this is necessary, too, in case the queue is full and not
|
||||
// started.
|
||||
int8_t addQueueEntry(const struct stepper_command_s* cmd, bool start = true);
|
||||
|
||||
// Return codes for addQueueEntry
|
||||
// positive values mean, that caller should retry later
|
||||
#define AQE_OK 0
|
||||
#define AQE_QUEUE_FULL 1
|
||||
#define AQE_DIR_PIN_IS_BUSY 2
|
||||
#define AQE_WAIT_FOR_ENABLE_PIN_ACTIVE 3
|
||||
#define AQE_DEVICE_NOT_READY 4
|
||||
#define AQE_ERROR_TICKS_TOO_LOW -1
|
||||
#define AQE_ERROR_EMPTY_QUEUE_TO_START -2
|
||||
#define AQE_ERROR_NO_DIR_PIN_TO_TOGGLE -3
|
||||
|
||||
// ### check functions for command queue being empty, full or running.
|
||||
bool isQueueEmpty() const;
|
||||
bool isQueueFull() const;
|
||||
bool isQueueRunning() const;
|
||||
|
||||
// ### functions to get the fill level of the queue
|
||||
//
|
||||
// To retrieve the forward planning time in the queue, ticksInQueue()
|
||||
// can be used. It sums up all ticks of the not yet processed commands.
|
||||
// For commands defining pauses, the summed up value is entry.ticks.
|
||||
// For commands with steps, the summed up value is entry.steps*entry.ticks
|
||||
uint32_t ticksInQueue();
|
||||
|
||||
// This function can be used to check, if the commands in the queue
|
||||
// will last for <min_ticks> ticks. This is again without the
|
||||
// currently processed command.
|
||||
bool hasTicksInQueue(uint32_t min_ticks) const;
|
||||
|
||||
// This function allows to check the number of commands in the queue.
|
||||
// This is including the currently processed command.
|
||||
uint8_t queueEntries() const;
|
||||
|
||||
// Get the future position of the stepper after all commands in queue are
|
||||
// completed
|
||||
int32_t getPositionAfterCommandsCompleted() const;
|
||||
|
||||
// Get the future speed of the stepper after all commands in queue are
|
||||
// completed. This is in µs. Returns 0 for stopped motor
|
||||
//
|
||||
// This value comes from the ramp generator and is not valid for raw command
|
||||
// queue
|
||||
// ==> Will be renamed in future release
|
||||
uint32_t getPeriodInUsAfterCommandsCompleted();
|
||||
uint32_t getPeriodInTicksAfterCommandsCompleted();
|
||||
|
||||
// Set the future position of the stepper after all commands in queue are
|
||||
// completed. This has immediate effect to getCurrentPosition().
|
||||
void setPositionAfterCommandsCompleted(int32_t new_pos);
|
||||
|
||||
// This function provides info, in which state the high level stepper control
|
||||
// is operating. The return value is an `or` of RAMP_STATE_... and
|
||||
// RAMP_DIRECTION_... flags. Definitions are above
|
||||
inline uint8_t rampState() { return _rg.rampState(); }
|
||||
|
||||
// returns true, if the ramp generation is active
|
||||
inline bool isRampGeneratorActive() { return _rg.isRampGeneratorActive(); }
|
||||
|
||||
// These functions allow to detach and reAttach a step pin for other use.
|
||||
// Pretty low level, use with care or not at all
|
||||
void detachFromPin() const;
|
||||
void reAttachToPin() const;
|
||||
|
||||
// ## ESP32 only: Free pulse counter
|
||||
// These four functions are only available on esp32.
|
||||
// The first can attach any of the eight pulse counters to this stepper.
|
||||
// The second then will read the current pulse counter value
|
||||
//
|
||||
// The user is responsible to not use a pulse counter, which is occupied by a
|
||||
// stepper and by this render the stepper or even blow up the uC.
|
||||
//
|
||||
// Pulse counter 6 and 7 are not used by the stepper library and are judged as
|
||||
// available. If only five steppers are defined, then 5 gets available. If
|
||||
// four steppers are defined, then 4 is usable,too.
|
||||
//
|
||||
// These functions are intended primarily for testing, because the library
|
||||
// should always output the correct amount of pulses. Possible application
|
||||
// usage would be an immediate and interrupt friendly version for
|
||||
// getCurrentPosition()
|
||||
//
|
||||
// The pulse counter counts up towards high_value.
|
||||
// If the high_value is reached, then the pulse counter is reset to 0.
|
||||
// Similarly, if direction pin is configured and set to count down,
|
||||
// then the pulse counter counts towards low_value. When the low value is hit,
|
||||
// the pulse counter is reset to 0.
|
||||
//
|
||||
// If low_value and high_value are set to zero, then the pulse counter is just
|
||||
// counting like any int16_t counter: 0...32767,-32768,-32767,...,0 and
|
||||
// backwards accordingly
|
||||
//
|
||||
// Possible application:
|
||||
// Assume the stepper, to which the pulse counter attached to, needs 3200
|
||||
// steps/revolution. If now attachToPulseCounter is called with -3200 and 3200
|
||||
// for the low and high values respectively, then the momentary angle of the
|
||||
// stepper (at exact this moment) can be retrieved just by reading the pulse
|
||||
// counter. If the value is negative, then just add 3200.
|
||||
//
|
||||
// Update for idf5 version:
|
||||
// The pcnt_unit value is not used, because the available units are managed
|
||||
// by the system. The parameter is kept for compatibility.
|
||||
//
|
||||
#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 5)
|
||||
bool attachToPulseCounter(uint8_t unused_pcnt_unit = 0,
|
||||
int16_t low_value = -16384,
|
||||
int16_t high_value = 16384);
|
||||
int16_t readPulseCounter();
|
||||
void clearPulseCounter();
|
||||
inline bool pulseCounterAttached() { return _attached_pulse_unit != NULL; }
|
||||
#endif
|
||||
#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 4)
|
||||
bool attachToPulseCounter(uint8_t pcnt_unit, int16_t low_value = -16384,
|
||||
int16_t high_value = 16384);
|
||||
int16_t readPulseCounter();
|
||||
void clearPulseCounter();
|
||||
inline bool pulseCounterAttached() { return _attached_pulse_cnt_unit >= 0; }
|
||||
#endif
|
||||
|
||||
private:
|
||||
void performOneStep(bool count_up, bool blocking = false);
|
||||
#ifdef SUPPORT_EXTERNAL_DIRECTION_PIN
|
||||
bool externalDirPinChangeCompletedIfNeeded();
|
||||
#endif
|
||||
void fill_queue();
|
||||
void updateAutoDisable();
|
||||
void blockingWaitForForceStopComplete();
|
||||
bool needAutoDisable();
|
||||
bool agreeWithAutoDisable();
|
||||
bool usesAutoEnablePin(uint8_t pin) const;
|
||||
void getCurrentSpeedInTicks(struct actual_ticks_s* speed, bool realtime);
|
||||
|
||||
FastAccelStepperEngine* _engine;
|
||||
RampGenerator _rg;
|
||||
uint8_t _stepPin;
|
||||
uint8_t _dirPin;
|
||||
bool _dirHighCountsUp;
|
||||
bool _autoEnable;
|
||||
uint8_t _enablePinLowActive;
|
||||
uint8_t _enablePinHighActive;
|
||||
uint8_t _queue_num;
|
||||
|
||||
uint16_t _dir_change_delay_ticks;
|
||||
uint32_t _on_delay_ticks;
|
||||
uint16_t _off_delay_count;
|
||||
uint16_t _auto_disable_delay_counter;
|
||||
|
||||
uint32_t _forward_planning_in_ticks;
|
||||
|
||||
#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 5)
|
||||
pcnt_unit_handle_t _attached_pulse_unit;
|
||||
#endif
|
||||
#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 4)
|
||||
int16_t _attached_pulse_cnt_unit;
|
||||
#endif
|
||||
#if (TEST_MEASURE_ISR_SINGLE_FILL == 1)
|
||||
uint32_t max_micros;
|
||||
#endif
|
||||
|
||||
friend class FastAccelStepperEngine;
|
||||
friend class FastAccelStepperTest;
|
||||
};
|
||||
|
||||
#endif
|
||||
95
src/FastAccelStepper_idf4_esp32_pcnt.cpp
Normal file
95
src/FastAccelStepper_idf4_esp32_pcnt.cpp
Normal file
@@ -0,0 +1,95 @@
|
||||
#include "StepperISR.h"
|
||||
#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 4)
|
||||
|
||||
uint32_t sig_idx[SUPPORT_ESP32_PULSE_COUNTER] = {
|
||||
PCNT_SIG_CH0_IN0_IDX, PCNT_SIG_CH0_IN1_IDX,
|
||||
PCNT_SIG_CH0_IN2_IDX, PCNT_SIG_CH0_IN3_IDX,
|
||||
#if SUPPORT_ESP32_PULSE_COUNTER == 8
|
||||
PCNT_SIG_CH0_IN4_IDX, PCNT_SIG_CH0_IN5_IDX,
|
||||
PCNT_SIG_CH0_IN6_IDX, PCNT_SIG_CH0_IN7_IDX,
|
||||
#endif
|
||||
};
|
||||
uint32_t ctrl_idx[SUPPORT_ESP32_PULSE_COUNTER] = {
|
||||
PCNT_CTRL_CH0_IN0_IDX, PCNT_CTRL_CH0_IN1_IDX, PCNT_CTRL_CH0_IN2_IDX,
|
||||
PCNT_CTRL_CH0_IN3_IDX,
|
||||
#if SUPPORT_ESP32_PULSE_COUNTER == 8
|
||||
PCNT_CTRL_CH0_IN4_IDX, PCNT_CTRL_CH0_IN5_IDX, PCNT_CTRL_CH0_IN6_IDX,
|
||||
PCNT_CTRL_CH0_IN7_IDX
|
||||
#endif
|
||||
};
|
||||
|
||||
bool FastAccelStepper::attachToPulseCounter(uint8_t pcnt_unit,
|
||||
int16_t low_value,
|
||||
int16_t high_value) {
|
||||
if (pcnt_unit >= SUPPORT_ESP32_PULSE_COUNTER) {
|
||||
return false;
|
||||
}
|
||||
|
||||
pcnt_config_t cfg;
|
||||
uint8_t dir_pin = getDirectionPin();
|
||||
uint8_t step_pin = getStepPin();
|
||||
cfg.pulse_gpio_num = PCNT_PIN_NOT_USED;
|
||||
if (dir_pin == PIN_UNDEFINED) {
|
||||
cfg.ctrl_gpio_num = PCNT_PIN_NOT_USED;
|
||||
cfg.hctrl_mode = PCNT_MODE_KEEP;
|
||||
cfg.lctrl_mode = PCNT_MODE_KEEP;
|
||||
} else {
|
||||
cfg.ctrl_gpio_num = dir_pin;
|
||||
if (directionPinHighCountsUp()) {
|
||||
cfg.lctrl_mode = PCNT_MODE_REVERSE;
|
||||
cfg.hctrl_mode = PCNT_MODE_KEEP;
|
||||
} else {
|
||||
cfg.lctrl_mode = PCNT_MODE_KEEP;
|
||||
cfg.hctrl_mode = PCNT_MODE_REVERSE;
|
||||
}
|
||||
}
|
||||
cfg.pos_mode = PCNT_COUNT_INC; // increment on rising edge
|
||||
cfg.neg_mode = PCNT_COUNT_DIS; // ignore falling edge
|
||||
cfg.counter_h_lim = high_value;
|
||||
cfg.counter_l_lim = low_value;
|
||||
cfg.unit = (pcnt_unit_t)pcnt_unit;
|
||||
cfg.channel = PCNT_CHANNEL_0;
|
||||
pcnt_unit_config(&cfg);
|
||||
|
||||
#ifndef HAVE_ESP32S3_PULSE_COUNTER
|
||||
PCNT.conf_unit[cfg.unit].conf0.thr_h_lim_en = 0;
|
||||
PCNT.conf_unit[cfg.unit].conf0.thr_l_lim_en = 0;
|
||||
#else
|
||||
PCNT.conf_unit[cfg.unit].conf0.thr_h_lim_en_un = 0;
|
||||
PCNT.conf_unit[cfg.unit].conf0.thr_l_lim_en_un = 0;
|
||||
#endif
|
||||
|
||||
// detachFromPin();
|
||||
// reAttachToPin();
|
||||
gpio_matrix_in(step_pin, sig_idx[pcnt_unit], 0);
|
||||
gpio_iomux_in(step_pin,
|
||||
sig_idx[pcnt_unit]); // test failure without this call
|
||||
if (dir_pin != PIN_UNDEFINED) {
|
||||
pinMode(dir_pin, OUTPUT);
|
||||
gpio_matrix_out(dir_pin, 0x100, false, false);
|
||||
gpio_matrix_in(dir_pin, ctrl_idx[pcnt_unit], 0);
|
||||
gpio_iomux_in(dir_pin, ctrl_idx[pcnt_unit]);
|
||||
}
|
||||
|
||||
pcnt_counter_clear(cfg.unit);
|
||||
pcnt_counter_resume(cfg.unit);
|
||||
|
||||
_attached_pulse_cnt_unit = pcnt_unit;
|
||||
return true;
|
||||
}
|
||||
void FastAccelStepper::clearPulseCounter() {
|
||||
if (pulseCounterAttached()) {
|
||||
pcnt_counter_clear((pcnt_unit_t)_attached_pulse_cnt_unit);
|
||||
}
|
||||
}
|
||||
int16_t FastAccelStepper::readPulseCounter() {
|
||||
if (pulseCounterAttached()) {
|
||||
#ifndef HAVE_ESP32S3_PULSE_COUNTER
|
||||
return PCNT.cnt_unit[(pcnt_unit_t)_attached_pulse_cnt_unit].cnt_val;
|
||||
#else
|
||||
return PCNT.cnt_unit[(pcnt_unit_t)_attached_pulse_cnt_unit].pulse_cnt_un;
|
||||
#endif
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
137
src/FastAccelStepper_idf5_esp32_pcnt.cpp
Normal file
137
src/FastAccelStepper_idf5_esp32_pcnt.cpp
Normal file
@@ -0,0 +1,137 @@
|
||||
#include "StepperISR.h"
|
||||
#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 5)
|
||||
|
||||
// Why the hell, does espressif think, that the unit and channel id are not
|
||||
// needed ? Without unit/channel ID, the needed parameter for
|
||||
// gpio_matrix_in/gpio_iomux_in cannot be derived.
|
||||
//
|
||||
// Here we declare the private pcnt_chan_t structure, which is not save.
|
||||
struct pcnt_unit_t {
|
||||
/*pcnt_group_t*/ void *group;
|
||||
portMUX_TYPE spinlock;
|
||||
int unit_id;
|
||||
// remainder of struct not needed
|
||||
};
|
||||
struct pcnt_chan_t {
|
||||
pcnt_unit_t *unit;
|
||||
int channel_id;
|
||||
// remainder of struct not needed
|
||||
};
|
||||
|
||||
bool FastAccelStepper::attachToPulseCounter(uint8_t unused_pcnt_unit,
|
||||
int16_t low_value,
|
||||
int16_t high_value) {
|
||||
pcnt_unit_config_t config = {.low_limit = low_value,
|
||||
.high_limit = high_value,
|
||||
.intr_priority = 0,
|
||||
.flags = {.accum_count = 0}};
|
||||
pcnt_unit_handle_t punit = NULL;
|
||||
esp_err_t rc;
|
||||
rc = pcnt_new_unit(&config, &punit);
|
||||
if (rc != ESP_OK) {
|
||||
return false;
|
||||
}
|
||||
|
||||
pcnt_chan_config_t chan_config = {.edge_gpio_num = -1,
|
||||
.level_gpio_num = -1,
|
||||
.flags = {
|
||||
.invert_edge_input = 0,
|
||||
.invert_level_input = 0,
|
||||
.virt_edge_io_level = 0,
|
||||
.virt_level_io_level = 0,
|
||||
.io_loop_back = 0,
|
||||
}};
|
||||
|
||||
pcnt_channel_level_action_t level_high = PCNT_CHANNEL_LEVEL_ACTION_KEEP;
|
||||
pcnt_channel_level_action_t level_low = PCNT_CHANNEL_LEVEL_ACTION_KEEP;
|
||||
uint8_t dir_pin = getDirectionPin();
|
||||
if (dir_pin != PIN_UNDEFINED) {
|
||||
chan_config.level_gpio_num = dir_pin;
|
||||
if (directionPinHighCountsUp()) {
|
||||
level_low = PCNT_CHANNEL_LEVEL_ACTION_INVERSE;
|
||||
} else {
|
||||
level_high = PCNT_CHANNEL_LEVEL_ACTION_INVERSE;
|
||||
}
|
||||
}
|
||||
|
||||
pcnt_channel_handle_t pcnt_chan = NULL;
|
||||
rc = pcnt_new_channel(punit, &chan_config, &pcnt_chan);
|
||||
if (rc != ESP_OK) {
|
||||
pcnt_del_unit(punit);
|
||||
return false;
|
||||
}
|
||||
|
||||
int unit_id = punit->unit_id;
|
||||
int channel_id = pcnt_chan->channel_id;
|
||||
if ((unit_id < 0) || (unit_id >= SUPPORT_ESP32_PULSE_COUNTER)) {
|
||||
// perhaps the pcnt_chan_t-structure is changed !?
|
||||
pcnt_del_channel(pcnt_chan);
|
||||
pcnt_del_unit(punit);
|
||||
return false;
|
||||
}
|
||||
|
||||
rc =
|
||||
pcnt_channel_set_edge_action(pcnt_chan, PCNT_CHANNEL_EDGE_ACTION_INCREASE,
|
||||
PCNT_CHANNEL_EDGE_ACTION_HOLD);
|
||||
if (rc != ESP_OK) {
|
||||
return false;
|
||||
}
|
||||
rc = pcnt_channel_set_level_action(pcnt_chan, level_high, level_low);
|
||||
if (rc != ESP_OK) {
|
||||
return false;
|
||||
}
|
||||
|
||||
rc = pcnt_unit_enable(punit);
|
||||
if (rc != ESP_OK) {
|
||||
return false;
|
||||
}
|
||||
rc = pcnt_unit_clear_count(punit);
|
||||
if (rc != ESP_OK) {
|
||||
return false;
|
||||
}
|
||||
rc = pcnt_unit_start(punit);
|
||||
if (rc != ESP_OK) {
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t step_pin = getStepPin();
|
||||
#ifdef TRACE
|
||||
printf("pins = %d/%d unit_id=%d channel_id=%d\n", step_pin, dir_pin, unit_id,
|
||||
channel_id);
|
||||
#endif
|
||||
int signal = pcnt_periph_signals.groups[0]
|
||||
.units[unit_id]
|
||||
.channels[channel_id]
|
||||
.pulse_sig;
|
||||
gpio_matrix_in(step_pin, signal, 0);
|
||||
gpio_iomux_in(step_pin, signal);
|
||||
if (dir_pin != PIN_UNDEFINED) {
|
||||
pinMode(dir_pin, OUTPUT);
|
||||
int control = pcnt_periph_signals.groups[0]
|
||||
.units[unit_id]
|
||||
.channels[channel_id]
|
||||
.control_sig;
|
||||
gpio_iomux_out(dir_pin, 0x100, false);
|
||||
gpio_matrix_in(dir_pin, control, 0);
|
||||
gpio_iomux_in(dir_pin, control);
|
||||
}
|
||||
|
||||
_attached_pulse_unit = punit;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void FastAccelStepper::clearPulseCounter() {
|
||||
if (pulseCounterAttached()) {
|
||||
pcnt_unit_clear_count(_attached_pulse_unit);
|
||||
}
|
||||
}
|
||||
int16_t FastAccelStepper::readPulseCounter() {
|
||||
int value = 0;
|
||||
if (pulseCounterAttached()) {
|
||||
pcnt_unit_get_count(_attached_pulse_unit, &value);
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
#endif
|
||||
302
src/PoorManFloat.cpp
Normal file
302
src/PoorManFloat.cpp
Normal file
@@ -0,0 +1,302 @@
|
||||
#include <stdint.h>
|
||||
#if defined(ARDUINO_ARCH_AVR)
|
||||
#include <avr/pgmspace.h>
|
||||
#else
|
||||
#define PROGMEM
|
||||
#define pgm_read_byte_near(x) (*(x))
|
||||
#endif
|
||||
#include "PoorManFloat.h"
|
||||
#ifdef TEST
|
||||
#include <stdio.h>
|
||||
#endif
|
||||
|
||||
// In FastAccelStepper there is seldom the need for adding or
|
||||
// subtracting floats, but multiplication/division/square and power
|
||||
// are often in use. Negative numbers and even zero are not needed.
|
||||
// Consequently, a purely logarithmic representation is completely
|
||||
// sufficient and the necessary range can be achieved by 16 bit signed integers.
|
||||
// The interpretation of a signed integer xi is:
|
||||
//
|
||||
// x = 2^(xi/512)
|
||||
//
|
||||
// The signed integer range xi (-32768..32767) is mapped to the
|
||||
// range (5e-20, 1.8e19). Please note: zero is not included.
|
||||
// The greatest constant in use is 2.2e14. So the range is sufficient.
|
||||
//
|
||||
// In order to map x to xi, log2(x) needs to be calculated.
|
||||
// For this we rewrite x to:
|
||||
// x = (1 + r) * 2^e
|
||||
//
|
||||
// With r being in the range 0 <= r < 1.
|
||||
// This means e is the largest integer value with 2^e < x.
|
||||
// Consequently, e can be derived by counting the leading numbers in
|
||||
// the integer x.
|
||||
//
|
||||
// So xi = 512*log2(x) = 512 * (e + log(1+r))
|
||||
//
|
||||
// The first 7 bits is plain e with sign and the lower 9 bits is log(1+r)*512
|
||||
// xi = eeee_eeem_mmmm_mmmm
|
||||
//
|
||||
// Example for 16 bit integer:
|
||||
// x = 15373 = 0b0011_1100_0000_1101
|
||||
//
|
||||
// log2(15373)*512 = 7121 (rounded)
|
||||
// = 0b0001_1011_1101_0001
|
||||
//
|
||||
// two leading zeros => e = 15 - 2 = 13 = 0b1101
|
||||
// 2^e = 8192
|
||||
//
|
||||
// => x = 8192 * 1.1_1100_0000_1101
|
||||
// r is the decimal part without the leading 1
|
||||
// r = 1_1100_0000 / 512
|
||||
// log2(1+r) * 512 = 464 = 0b1_1101_0001
|
||||
//
|
||||
// xi = eeee_eeem_mmmm_mmmm
|
||||
// = 0001_1011_1101_0001
|
||||
// = 0x1bd1
|
||||
//
|
||||
// The remaining task is to calculate log2(1+f).
|
||||
//
|
||||
// log2(1+r) is at the corners identical to r:
|
||||
// log2(1+0) = 0 and log2(1+1) = 1
|
||||
// So it is interesting to look at function
|
||||
// f(1+r) = log2(1+r) - r
|
||||
//
|
||||
// This function is 0 for r at 0 and 1, positive over the range inbetween
|
||||
// and reaches max value of 0.08607 at r = 0.442695.
|
||||
// This max value is at r = 1/ln(2)-1
|
||||
//
|
||||
// As we need actually 512*log2(1+r), then the max value is 44.
|
||||
// This allows to multiply with up to 8 to improve the resolution.
|
||||
// Here we use factor 4, so two table values can be summed up without overflow.
|
||||
//
|
||||
// So the log2 can be calculated for e.g.:
|
||||
// x = 0001_mmmm_mmmm_mmmm three leading zeros
|
||||
//
|
||||
// Shift left by leading zeros+1 (removing leading 1) and then right by 5:
|
||||
// x = 0001_mmmm_mmmm_mmmm three leading zeros
|
||||
// mmmm_mmmm_mmmm_m000 shift left (3+1)
|
||||
// 0000_0mmm_mmmm_mmmm shift right (5)
|
||||
// if ninth bit is 0:
|
||||
// + ttt_tttt0 add table value for mmmm_mmmm shifted one
|
||||
// else:
|
||||
// + 0ttt_tttt add table value for mmmm_mmmm
|
||||
// + 0ttt_tttt add table value for mmmm_mmmm+1
|
||||
// + 0000_0001 round
|
||||
// result: mmmm_mmmm_m
|
||||
// final: 000e_eeem_mmmm_mmmm For up to 32bits (e = 31)
|
||||
//
|
||||
// We are using a table of length 256, so first 8 bits are the index.
|
||||
// In order to improve the resolution, the ninth bit is used to interpolate
|
||||
// with the next table entry.
|
||||
//
|
||||
// Using python3 this can be calculated by:
|
||||
// [round(math.log2(i/256) * 256 - (i-256)) for i in range(256,512)]
|
||||
//
|
||||
// For better precision y_yyyy is shifted by 2 and can be calculated as:
|
||||
// [round((math.log2(i/256) * 256 - (i-256))*4) for i in range(256,512)]
|
||||
//
|
||||
const PROGMEM uint8_t log2_minus_x_plus_one_shifted_by_2[256] = {
|
||||
0, 2, 3, 5, 7, 9, 10, 12, 13, 15, 17, 18, 20, 21, 23, 24, 26, 27, 28,
|
||||
30, 31, 32, 34, 35, 36, 38, 39, 40, 41, 43, 44, 45, 46, 47, 48, 49, 50, 51,
|
||||
52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 64, 65, 66, 67, 68, 68,
|
||||
69, 70, 70, 71, 72, 72, 73, 74, 74, 75, 75, 76, 77, 77, 78, 78, 79, 79, 80,
|
||||
80, 80, 81, 81, 82, 82, 83, 83, 83, 84, 84, 84, 84, 85, 85, 85, 86, 86, 86,
|
||||
86, 86, 87, 87, 87, 87, 87, 87, 88, 88, 88, 88, 88, 88, 88, 88, 88, 88, 88,
|
||||
88, 88, 88, 88, 88, 88, 88, 88, 88, 88, 88, 87, 87, 87, 87, 87, 87, 86, 86,
|
||||
86, 86, 86, 85, 85, 85, 85, 84, 84, 84, 84, 83, 83, 83, 82, 82, 82, 81, 81,
|
||||
81, 80, 80, 79, 79, 79, 78, 78, 77, 77, 76, 76, 75, 75, 74, 74, 73, 73, 72,
|
||||
72, 71, 71, 70, 70, 69, 68, 68, 67, 67, 66, 65, 65, 64, 63, 63, 62, 61, 61,
|
||||
60, 59, 59, 58, 57, 57, 56, 55, 54, 54, 53, 52, 51, 51, 50, 49, 48, 47, 47,
|
||||
46, 45, 44, 43, 42, 42, 41, 40, 39, 38, 37, 36, 35, 34, 34, 33, 32, 31, 30,
|
||||
29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 19, 18, 17, 16, 15, 14, 13, 12, 11,
|
||||
10, 9, 8, 7, 6, 4, 3, 2, 1};
|
||||
|
||||
// For the inverse pow(2,x) needs to be calculated. Similarly it makes sense to
|
||||
// evaluate instead
|
||||
// g(x) = x - pow(2,x-1)
|
||||
//
|
||||
// This function equals 0 for x at 1 and 2, with extremum 0.08607 at x
|
||||
// = 1.528766. This max. value is at x = 1 - ln(ln(2))/ln(2)
|
||||
//
|
||||
// Noteworthy the max values of log2(x) - x + 1 and x - pow(2, x-1) are
|
||||
// identical, calculated by (-1 + ln(2) - ln(ln(2)))/ln(2)
|
||||
//
|
||||
// Using python3 this can be calculated by:
|
||||
// [round(i - math.pow(2,i/256-1)*256) for i in range(256,512)]
|
||||
//
|
||||
// Similarly shifted by two bits:
|
||||
// [round((i - math.pow(2,i/256-1)*256)*4) for i in range(256,512)]
|
||||
//
|
||||
const PROGMEM uint8_t x_minus_pow2_of_x_minus_one_shifted_by_2[256] = {
|
||||
0, 1, 2, 4, 5, 6, 7, 8, 10, 11, 12, 13, 14, 15, 16, 18, 19, 20, 21,
|
||||
22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40,
|
||||
41, 42, 43, 44, 45, 46, 46, 47, 48, 49, 50, 51, 52, 52, 53, 54, 55, 56, 56,
|
||||
57, 58, 59, 59, 60, 61, 62, 62, 63, 64, 64, 65, 66, 66, 67, 68, 68, 69, 69,
|
||||
70, 71, 71, 72, 72, 73, 73, 74, 74, 75, 76, 76, 76, 77, 77, 78, 78, 79, 79,
|
||||
80, 80, 80, 81, 81, 82, 82, 82, 83, 83, 83, 84, 84, 84, 84, 85, 85, 85, 85,
|
||||
86, 86, 86, 86, 87, 87, 87, 87, 87, 87, 87, 88, 88, 88, 88, 88, 88, 88, 88,
|
||||
88, 88, 88, 88, 88, 88, 88, 88, 88, 88, 88, 88, 88, 88, 87, 87, 87, 87, 87,
|
||||
87, 86, 86, 86, 86, 86, 85, 85, 85, 84, 84, 84, 84, 83, 83, 83, 82, 82, 81,
|
||||
81, 81, 80, 80, 79, 79, 78, 78, 77, 77, 76, 76, 75, 75, 74, 74, 73, 72, 72,
|
||||
71, 71, 70, 69, 68, 68, 67, 66, 66, 65, 64, 63, 63, 62, 61, 60, 59, 58, 58,
|
||||
57, 56, 55, 54, 53, 52, 51, 50, 49, 48, 47, 46, 45, 44, 43, 42, 41, 40, 39,
|
||||
38, 36, 35, 34, 33, 32, 30, 29, 28, 27, 25, 24, 23, 22, 20, 19, 17, 16, 15,
|
||||
13, 12, 10, 9, 8, 6, 5, 3, 2};
|
||||
|
||||
uint8_t leading_zeros(uint8_t x) {
|
||||
uint8_t res;
|
||||
if ((x & 0xf0) == 0) {
|
||||
x <<= 4;
|
||||
res = 4;
|
||||
} else {
|
||||
res = 0;
|
||||
}
|
||||
if ((x & 0xc0) == 0) {
|
||||
x <<= 2;
|
||||
res += 2;
|
||||
}
|
||||
if ((x & 0x80) == 0) {
|
||||
res += 1;
|
||||
if (x == 0) {
|
||||
res += 1;
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
pmf_logarithmic pmfl_from(uint8_t x) {
|
||||
// calling with x == 0 is considered an error.
|
||||
//
|
||||
// In a first step convert to
|
||||
// 0000_0eee_mmmm_mmmm
|
||||
//
|
||||
// Hereby mmmm_mmmm are the lower bits right from the first 1 in x
|
||||
// An example with only four valid mantissa bits:
|
||||
// x = 0001_mmmm => 0000_0100_mmmm_0000
|
||||
//
|
||||
// eee is the exponent
|
||||
//
|
||||
// The second convert this to the logarithm of x
|
||||
// 1. Use mmmm_mmmm as index in the log2_minus_x_plus_one_shifted_by_1
|
||||
// table
|
||||
// 2. shift left 0000_0eee_mmmm_mmmm by 1
|
||||
// 3. add the value from the log2_minus_x_plus_one_shifted_by_1 table
|
||||
uint8_t leading = leading_zeros(x);
|
||||
if (leading == 8) {
|
||||
return PMF_CONST_INVALID;
|
||||
}
|
||||
x <<= leading + 1;
|
||||
uint8_t e = 7 - leading;
|
||||
uint8_t offset = pgm_read_byte_near(&log2_minus_x_plus_one_shifted_by_2[x]);
|
||||
uint16_t res = (((uint16_t)e) << 8) | x;
|
||||
res <<= 1;
|
||||
offset += 1;
|
||||
res += offset >> 1;
|
||||
return res;
|
||||
}
|
||||
|
||||
pmf_logarithmic pmfl_from(uint16_t x) {
|
||||
uint8_t leading = leading_zeros(x >> 8);
|
||||
if (leading == 8) {
|
||||
return pmfl_from((uint8_t)x);
|
||||
}
|
||||
// shift msb out
|
||||
x <<= leading + 1;
|
||||
uint8_t exponent = 15 - leading;
|
||||
x >>= 5;
|
||||
uint8_t index = x >> 3;
|
||||
uint8_t offset =
|
||||
pgm_read_byte_near(&log2_minus_x_plus_one_shifted_by_2[index]);
|
||||
// only with x & 7 > 2, the calculated constants are correct...
|
||||
if ((x & 7) > 2) {
|
||||
index++; // overflow to 0 is ok. index is an uint8_t
|
||||
offset += pgm_read_byte_near(&log2_minus_x_plus_one_shifted_by_2[index]);
|
||||
offset += 1;
|
||||
} else {
|
||||
offset <<= 1;
|
||||
// offset += 1;
|
||||
}
|
||||
x += offset;
|
||||
x >>= 2;
|
||||
x += ((uint16_t)exponent) << 9;
|
||||
return x;
|
||||
}
|
||||
pmf_logarithmic pmfl_from(uint32_t x) {
|
||||
int16_t exp_offset;
|
||||
uint16_t w;
|
||||
if ((x & 0xff000000) == 0) {
|
||||
if ((x & 0x00ff0000) == 0) {
|
||||
w = (uint16_t)x;
|
||||
exp_offset = 0;
|
||||
} else if ((x & 0x00f00000) == 0) {
|
||||
w = x >> 4;
|
||||
exp_offset = 0x0800;
|
||||
} else {
|
||||
w = x >> 8;
|
||||
exp_offset = 0x1000;
|
||||
}
|
||||
} else if ((x & 0xf0000000) == 0) {
|
||||
w = x >> 12;
|
||||
exp_offset = 0x1800;
|
||||
} else {
|
||||
w = x >> 16;
|
||||
exp_offset = 0x2000;
|
||||
}
|
||||
return pmfl_from(w) + exp_offset;
|
||||
}
|
||||
uint16_t pmfl_to_u16(pmf_logarithmic x) {
|
||||
if (x < 0) {
|
||||
return 0;
|
||||
}
|
||||
if (x >= PMF_CONST_UINT16_MAX) {
|
||||
return __UINT16_MAX__;
|
||||
}
|
||||
uint8_t exponent = ((uint16_t)x) >> 9;
|
||||
x &= 0x01ff;
|
||||
x += 0x200;
|
||||
uint8_t index = ((uint16_t)x) >> 1;
|
||||
x <<= 1;
|
||||
uint8_t offset =
|
||||
pgm_read_byte_near(&x_minus_pow2_of_x_minus_one_shifted_by_2[index]);
|
||||
if ((x & 2) != 0) {
|
||||
index++; // overflow to 0 is ok. index is an uint8_t
|
||||
offset +=
|
||||
pgm_read_byte_near(&x_minus_pow2_of_x_minus_one_shifted_by_2[index]);
|
||||
offset >>= 1;
|
||||
}
|
||||
x -= offset;
|
||||
if (exponent > 10) {
|
||||
x <<= exponent - 10;
|
||||
} else if (exponent < 10) {
|
||||
x += (exponent != 9) ? 2 : 1;
|
||||
x >>= 10 - exponent;
|
||||
}
|
||||
return x;
|
||||
}
|
||||
uint32_t pmfl_to_u32(pmf_logarithmic x) {
|
||||
if (x < 0) {
|
||||
return 0;
|
||||
}
|
||||
if (x >= PMF_CONST_UINT32_MAX) {
|
||||
return __UINT32_MAX__;
|
||||
}
|
||||
uint8_t exponent = ((uint16_t)x) >> 9;
|
||||
if (exponent < 0x10) {
|
||||
return pmfl_to_u16(x);
|
||||
}
|
||||
uint8_t shift = exponent - 0x0f;
|
||||
x = pmfl_shr(x, shift);
|
||||
uint32_t res = pmfl_to_u16(x);
|
||||
res <<= shift;
|
||||
return res;
|
||||
}
|
||||
pmf_logarithmic pmfl_square(pmf_logarithmic x) {
|
||||
if (x > 0x4000) {
|
||||
return PMF_CONST_MAX;
|
||||
}
|
||||
if (x <= -0x4000) {
|
||||
return PMF_CONST_MIN;
|
||||
}
|
||||
return x + x;
|
||||
}
|
||||
45
src/PoorManFloat.h
Normal file
45
src/PoorManFloat.h
Normal file
@@ -0,0 +1,45 @@
|
||||
#ifndef POORMANFLOAT_H
|
||||
#define POORMANFLOAT_H
|
||||
#include <stdint.h>
|
||||
|
||||
#include <PoorManFloatConst.h>
|
||||
|
||||
typedef int16_t pmf_logarithmic;
|
||||
|
||||
#define PMF_CONST_INVALID ((pmf_logarithmic)0x8000)
|
||||
#define PMF_CONST_MAX ((pmf_logarithmic)0x7fff)
|
||||
#define PMF_CONST_MIN ((pmf_logarithmic)0x8001)
|
||||
|
||||
pmf_logarithmic pmfl_from(uint8_t x);
|
||||
pmf_logarithmic pmfl_from(uint16_t x);
|
||||
pmf_logarithmic pmfl_from(uint32_t x);
|
||||
|
||||
uint16_t pmfl_to_u16(pmf_logarithmic x);
|
||||
uint32_t pmfl_to_u32(pmf_logarithmic x);
|
||||
|
||||
#define pmfl_shl(x, n) ((x) + (((int16_t)(n)) << 9))
|
||||
#define pmfl_shr(x, n) ((x) - (((int16_t)(n)) << 9))
|
||||
|
||||
#define pmfl_multiply(x, y) ((x) + (y))
|
||||
#define pmfl_divide(x, y) ((x) - (y))
|
||||
#define pmfl_reciprocal(x) (-(x))
|
||||
#define pmfl_sqrt(x) ((x) / 2)
|
||||
#define pmfl_rsqrt(x) (-(x) / 2)
|
||||
#define pmfl_rsquare(x) pmfl_reciprocal(pmfl_square(x))
|
||||
|
||||
inline pmf_logarithmic pmfl_pow_div_3(pmf_logarithmic x) {
|
||||
// 1/3 ~ (1/4+1/16+1/64+1/256+1/1024+1/4096+1/16384)
|
||||
x /= 2; // x/2
|
||||
x += x / 4; // x/2 + x/8
|
||||
x += x / 16; // x/2 + x/8 + x/32 + x/128
|
||||
x += x / 256; // x/2 + x/8 + x/32 + x/128 + x/512 + x/2048 + x/8192 + x/32768
|
||||
x += 1;
|
||||
return x / 2;
|
||||
}
|
||||
#define pmfl_pow_2_div_3(x) ((x) - pmfl_pow_div_3(x))
|
||||
#define pmfl_pow_3_div_2(x) ((x) + (x) / 2)
|
||||
|
||||
pmf_logarithmic pmfl_square(pmf_logarithmic x);
|
||||
|
||||
uint8_t leading_zeros(uint8_t x);
|
||||
#endif
|
||||
73
src/PoorManFloatConst.h
Normal file
73
src/PoorManFloatConst.h
Normal file
@@ -0,0 +1,73 @@
|
||||
// Autogenerated by extras/gen_pmf_const/main
|
||||
// DO NOT EDIT
|
||||
#ifndef POORMANFLOATCONST_H
|
||||
#define POORMANFLOATCONST_H
|
||||
|
||||
#include <PoorManFloat.h>
|
||||
|
||||
// PMF_CONST_1 = 0.000000 0 = 0x0000
|
||||
#define PMF_CONST_1 ((pmf_logarithmic)0x0000)
|
||||
// => converted back => 1.00
|
||||
|
||||
// PMF_CONST_3_DIV_2 = 299.500793 300 = 0x012c
|
||||
#define PMF_CONST_3_DIV_2 ((pmf_logarithmic)0x012c)
|
||||
// => converted back => 1.50
|
||||
|
||||
// PMF_CONST_128E12 = 23993.925781 23994 = 0x5dba
|
||||
#define PMF_CONST_128E12 ((pmf_logarithmic)0x5dba)
|
||||
// => converted back => 128012783714304.00
|
||||
|
||||
// PMF_CONST_16E6 = 12252.962891 12253 = 0x2fdd
|
||||
#define PMF_CONST_16E6 ((pmf_logarithmic)0x2fdd)
|
||||
// => converted back => 16000799.00
|
||||
|
||||
// PMF_CONST_500 = 4590.481445 4590 = 0x11ee
|
||||
#define PMF_CONST_500 ((pmf_logarithmic)0x11ee)
|
||||
// => converted back => 499.67
|
||||
|
||||
// PMF_CONST_1000 = 5102.481445 5102 = 0x13ee
|
||||
#define PMF_CONST_1000 ((pmf_logarithmic)0x13ee)
|
||||
// => converted back => 999.35
|
||||
|
||||
// PMF_CONST_2000 = 5614.481445 5614 = 0x15ee
|
||||
#define PMF_CONST_2000 ((pmf_logarithmic)0x15ee)
|
||||
// => converted back => 1998.70
|
||||
|
||||
// PMF_CONST_32000 = 7662.481445 7662 = 0x1dee
|
||||
#define PMF_CONST_32000 ((pmf_logarithmic)0x1dee)
|
||||
// => converted back => 31979.14
|
||||
|
||||
// PMF_CONST_16E6_DIV_SQRT_OF_2 = 11996.962891 11997 = 0x2edd
|
||||
#define PMF_CONST_16E6_DIV_SQRT_OF_2 ((pmf_logarithmic)0x2edd)
|
||||
// => converted back => 11314274.00
|
||||
|
||||
// PMF_CONST_21E6 = 12453.830078 12454 = 0x30a6
|
||||
#define PMF_CONST_21E6 ((pmf_logarithmic)0x30a6)
|
||||
// => converted back => 21004844.00
|
||||
|
||||
// PMF_CONST_42000 = 7863.348145 7863 = 0x1eb7
|
||||
#define PMF_CONST_42000 ((pmf_logarithmic)0x1eb7)
|
||||
// => converted back => 41980.21
|
||||
|
||||
// PMF_CONST_21E6_DIV_SQRT_OF_2 = 12197.830078 12198 = 0x2fa6
|
||||
#define PMF_CONST_21E6_DIV_SQRT_OF_2 ((pmf_logarithmic)0x2fa6)
|
||||
// => converted back => 14852668.00
|
||||
|
||||
// PMF_CONST_2205E11 = 24395.660156 24396 = 0x5f4c
|
||||
#define PMF_CONST_2205E11 ((pmf_logarithmic)0x5f4c)
|
||||
// => converted back => 220601734135808.00
|
||||
|
||||
// PMF_CONST_UINT32_MAX = 16384.000000 16384 = 0x4000
|
||||
#define PMF_CONST_UINT32_MAX ((pmf_logarithmic)0x4000)
|
||||
// => converted back => 4294967296.00
|
||||
|
||||
// PMF_CONST_UINT16_MAX = 8191.988770 8192 = 0x2000
|
||||
#define PMF_CONST_UINT16_MAX ((pmf_logarithmic)0x2000)
|
||||
// => converted back => 65536.00
|
||||
|
||||
// used in PoorManFloat.cpp as example
|
||||
//
|
||||
// PMF_CONST_15373 = 7120.953125 7121 = 0x1bd1
|
||||
#define PMF_CONST_15373 ((pmf_logarithmic)0x1bd1)
|
||||
// => converted back => 15373.98
|
||||
#endif
|
||||
181
src/RampCalculator.cpp
Normal file
181
src/RampCalculator.cpp
Normal file
@@ -0,0 +1,181 @@
|
||||
#include "RampCalculator.h"
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#include "StepperISR.h"
|
||||
|
||||
#ifdef TEST_TIMING
|
||||
// This module has only one purpose:
|
||||
//
|
||||
// - To calculate for a given step the corresponding speed.
|
||||
//
|
||||
// With constant acceleration the relation between the steps s. acceleration a,
|
||||
// time t and speed v will fulfill this equation during acceleration and
|
||||
// deceleration:
|
||||
//
|
||||
// s = 0.5 * a * t² = 0.5 * v² / a
|
||||
//
|
||||
// Acceleration is just counting up the steps and deceleration counting down
|
||||
// towards 0.
|
||||
//
|
||||
// From this equation the actual speed v at a given step can be calculated by:
|
||||
//
|
||||
// v = sqrt(2*s*a)
|
||||
//
|
||||
// For the pwm, the speed needs to be translated into time ticks T = n *
|
||||
// timeticks = 1/v:
|
||||
//
|
||||
// n * timeticks = 1 / sqrt(2*s*a)
|
||||
//
|
||||
// If the µC would be fast, then the solution would be in floating point:
|
||||
uint32_t calculate_ticks_v1(uint32_t steps, float acceleration) {
|
||||
float n = TICKS_PER_S / sqrt(2.0 * steps * acceleration);
|
||||
return n;
|
||||
}
|
||||
// Just this takes approx 92-4 = 88 us. Which is pretty slow.
|
||||
//
|
||||
// Little optimization improves to 87-4 = 83 us
|
||||
uint32_t calculate_ticks_v2(uint32_t steps, float acceleration) {
|
||||
float n = TICKS_PER_S / sqrt(2 * steps * acceleration);
|
||||
return n;
|
||||
}
|
||||
//
|
||||
// Precalculate TICKS_PER_S / sqrt(2 * acceleration)
|
||||
// results in 80-4 = 76us
|
||||
uint32_t calculate_ticks_v3(uint32_t steps, float pre_calc) {
|
||||
float n = pre_calc / sqrt(steps);
|
||||
return n;
|
||||
}
|
||||
//
|
||||
// Using pmf_logarithmic improves to 22-4 = 18us, but less precision
|
||||
uint32_t calculate_ticks_v4(uint32_t steps, uint32_t acceleration) {
|
||||
pmf_logarithmic pmfl_a = pmfl_from(acceleration);
|
||||
pmf_logarithmic pmfl_s = pmfl_from(steps);
|
||||
pmf_logarithmic pmfl_res =
|
||||
pmfl_divide(PMF_TICKS_PER_S, pmfl_sqrt(pmfl_multiply(pmfl_s, pmfl_a)));
|
||||
uint32_t res = pmfl_to_u32(pmfl_res);
|
||||
return res;
|
||||
}
|
||||
//
|
||||
// Precalculating the acceleration related constant improves further to 12-4 =
|
||||
// 8us. This measurement was actually done with ump_float.
|
||||
// So this routine does not fit to that measurement anymore
|
||||
//
|
||||
uint32_t calculate_ticks_v5(uint32_t steps, pmf_logarithmic pre_calc) {
|
||||
pmf_logarithmic pmfl_s = pmfl_from(steps);
|
||||
pmf_logarithmic pmfl_res = pmfl_divide(pre_calc, pmfl_sqrt(pmfl_s));
|
||||
uint32_t res = pmfl_to_u32(pmfl_res);
|
||||
return res;
|
||||
}
|
||||
//
|
||||
// using the combined function yields actually no measureable improvement
|
||||
uint32_t calculate_ticks_v6(uint32_t steps, pmf_logarithmic pre_calc) {
|
||||
// pmf_logarithmic pmfl_s = pmfl_from(steps);
|
||||
// pmf_logarithmic pmfl_res = pmfl_sqrt_after_divide(pre_calc, pmfl_s);
|
||||
// uint32_t res = pmfl_to_u32(pmfl_res);
|
||||
// return res;
|
||||
return calculate_ticks_v5(steps, pre_calc);
|
||||
}
|
||||
//
|
||||
// In order to increase the precision to more than 8 bits and avoid
|
||||
// the penalty of slow float operations different approach will be used.
|
||||
//
|
||||
// in order to avoid division+sqrt operation, the following equation will be
|
||||
// rearranged:
|
||||
//
|
||||
// n * timeticks = 1 / sqrt(2*s*a)
|
||||
//
|
||||
// into:
|
||||
//
|
||||
// n² * s = 1 / sqrt(2*a) / timeticks²
|
||||
//
|
||||
// with the right part being constant for a given acceleration a
|
||||
//
|
||||
// n² * s = const(a)
|
||||
//
|
||||
// This can be solved iteratively
|
||||
//
|
||||
// n_0 = 0
|
||||
// n_i
|
||||
// n_[i+1] = { with n_i² * s <= const(a) < (n_i
|
||||
// + mask_i)² * s
|
||||
// n_i + mask_i
|
||||
//
|
||||
// mask_[i+1] = mask_i >> 1
|
||||
//
|
||||
// In order to avoid the square calculation in each step several temporary
|
||||
// values need to be calculated
|
||||
// n_i² * s = f_i
|
||||
// f_[i+1] = n_[i+1]² * s = { with same condition
|
||||
// as before
|
||||
// (n_i + mask_i)² * s
|
||||
//
|
||||
// For the lower case the calculation continues:
|
||||
//
|
||||
// = n_i² * s + 2 * n_i * mask_i * s + mask_i² * s
|
||||
//
|
||||
// = f_i + g_i + h_i
|
||||
//
|
||||
// With g_i and h_i defined like this:
|
||||
//
|
||||
// g_0 = 0
|
||||
//
|
||||
// g_[i+1] = 2 * n_[i+1] * mask_[i+1] * s
|
||||
//
|
||||
// 2 * n_i * (mask_i >> 1) * s = g_i >> 1
|
||||
// = {
|
||||
// 2 * (n_i + mask_i) * (mask_i >> 1) * s =
|
||||
//(g_i
|
||||
//>> 1) + (mask_i² * s)
|
||||
// = (g_i >> 1) + h_i
|
||||
//
|
||||
// h_0 = mask_0 ² * s
|
||||
//
|
||||
// h_[i+1] = mask_[i+1]² * s
|
||||
// = (mask_i >> 1)² * s
|
||||
// = h_i >> 2
|
||||
//
|
||||
// Actually this is a dead end, because below routines already needs 35us,
|
||||
// while several variables below ought to be 32bit
|
||||
uint32_t calculate_ticks_v7(uint32_t steps, pmf_logarithmic pre_calc) {
|
||||
// initial values for i = 0
|
||||
uint16_t mask_i = 0x800;
|
||||
uint16_t n_i = 0;
|
||||
uint16_t f_i = 0;
|
||||
uint16_t g_i = 0;
|
||||
uint16_t h_i = steps;
|
||||
uint16_t const_a = pre_calc;
|
||||
|
||||
while (mask_i) {
|
||||
uint16_t f_x = f_i + g_i + h_i;
|
||||
// Serial.println(f_x);
|
||||
if (f_x < const_a) {
|
||||
n_i += mask_i;
|
||||
f_i = f_x;
|
||||
g_i >>= 1;
|
||||
g_i += h_i;
|
||||
h_i >>= 2;
|
||||
mask_i >>= 1;
|
||||
} else {
|
||||
g_i >>= 1;
|
||||
h_i >>= 2;
|
||||
mask_i >>= 1;
|
||||
}
|
||||
}
|
||||
return n_i;
|
||||
}
|
||||
|
||||
// This is used and requires 9.5us.
|
||||
//
|
||||
// Measure with:
|
||||
// cd extras/tests/simavr_based
|
||||
// make -C off_test_timing
|
||||
// From the results subtract port B from port A measurement
|
||||
uint32_t calculate_ticks_v8(uint32_t steps, pmf_logarithmic pre_calc) {
|
||||
pmf_logarithmic pmfl_steps = pmfl_from(steps);
|
||||
pmf_logarithmic pmfl_sqrt_steps = pmfl_sqrt(pmfl_steps);
|
||||
pmf_logarithmic pmfl_res = pmfl_divide(pre_calc, pmfl_sqrt_steps);
|
||||
uint32_t res = pmfl_to_u32(pmfl_res);
|
||||
return res;
|
||||
}
|
||||
#endif
|
||||
235
src/RampCalculator.h
Normal file
235
src/RampCalculator.h
Normal file
@@ -0,0 +1,235 @@
|
||||
#ifndef RAMP_CALCULATOR_H
|
||||
#define RAMP_CALCULATOR_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "PoorManFloat.h"
|
||||
#include "fas_arch/common.h"
|
||||
|
||||
#if (TICKS_PER_S == 16000000L)
|
||||
#define PMF_TICKS_PER_S PMF_CONST_16E6
|
||||
#define PMF_TICKS_PER_S_DIV_SQRT_OF_2 PMF_CONST_16E6_DIV_SQRT_OF_2
|
||||
#define PMF_ACCEL_FACTOR PMF_CONST_128E12
|
||||
#define US_TO_TICKS(u32) ((u32) * 16)
|
||||
#define TICKS_TO_US(u32) ((u32) / 16)
|
||||
#elif (TICKS_PER_S == 21000000L)
|
||||
#define PMF_TICKS_PER_S PMF_CONST_21E6
|
||||
#define PMF_TICKS_PER_S_DIV_SQRT_OF_2 PMF_CONST_21E6_DIV_SQRT_OF_2
|
||||
#define PMF_ACCEL_FACTOR PMF_CONST_2205E11
|
||||
#define US_TO_TICKS(u32) ((u32) * 21)
|
||||
#define TICKS_TO_US(u32) ((u32) / 21)
|
||||
#else
|
||||
#define SUPPORT_PMF_TIMER_FREQ_VARIABLES
|
||||
#define PMF_TICKS_PER_S pmfl_timer_freq
|
||||
#define PMF_TICKS_PER_S_DIV_SQRT_OF_2 pmfl_timer_freq_div_sqrt_of_2
|
||||
#define PMF_ACCEL_FACTOR pmfl_timer_freq_square_div_2
|
||||
// This overflows for approx. 1s at 40 MHz, only
|
||||
#define US_TO_TICKS(u32) \
|
||||
((uint32_t)((((uint32_t)((u32) * (TICKS_PER_S / 10000L))) / 100L)))
|
||||
|
||||
// This calculation needs more work
|
||||
#define TICKS_TO_US(u32) \
|
||||
((uint32_t)((((uint32_t)((u32) / (TICKS_PER_S / 1000000L))) / 1L)))
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef TEST_TIMING
|
||||
uint32_t calculate_ticks_v1(uint32_t steps, float acceleration);
|
||||
uint32_t calculate_ticks_v2(uint32_t steps, float acceleration);
|
||||
uint32_t calculate_ticks_v3(uint32_t steps, float pre_calc);
|
||||
uint32_t calculate_ticks_v4(uint32_t steps, uint32_t acceleration);
|
||||
uint32_t calculate_ticks_v5(uint32_t steps, pmf_logarithmic pre_calc);
|
||||
uint32_t calculate_ticks_v6(uint32_t steps, pmf_logarithmic pre_calc);
|
||||
uint32_t calculate_ticks_v7(uint32_t steps, pmf_logarithmic pre_calc);
|
||||
uint32_t calculate_ticks_v8(uint32_t steps, pmf_logarithmic pre_calc);
|
||||
#endif
|
||||
|
||||
struct ramp_parameters_s {
|
||||
int32_t move_value;
|
||||
uint32_t min_travel_ticks;
|
||||
uint32_t s_h;
|
||||
uint32_t s_jump;
|
||||
pmf_logarithmic pmfl_accel;
|
||||
bool apply : 1; // clear on read by stepper task. Triggers read !
|
||||
bool any_change : 1; // clear on read by stepper task
|
||||
bool recalc_ramp_steps : 1; // clear on read by stepper task
|
||||
bool valid_acceleration : 1;
|
||||
bool valid_speed : 1;
|
||||
bool move_absolute : 1;
|
||||
bool keep_running : 1;
|
||||
bool keep_running_count_up : 1;
|
||||
|
||||
void init() {
|
||||
move_value = 0;
|
||||
valid_acceleration = false;
|
||||
valid_speed = false;
|
||||
apply = false;
|
||||
any_change = false;
|
||||
recalc_ramp_steps = false;
|
||||
move_absolute = true;
|
||||
keep_running = false;
|
||||
keep_running_count_up = true;
|
||||
s_h = 0;
|
||||
s_jump = 0;
|
||||
min_travel_ticks = 0;
|
||||
}
|
||||
inline void applyParameters() {
|
||||
if (any_change) {
|
||||
fasDisableInterrupts();
|
||||
apply = true;
|
||||
fasEnableInterrupts();
|
||||
}
|
||||
}
|
||||
inline void setRunning(bool count_up) {
|
||||
fasDisableInterrupts();
|
||||
keep_running = true;
|
||||
keep_running_count_up = count_up;
|
||||
any_change = true;
|
||||
apply = true;
|
||||
fasEnableInterrupts();
|
||||
}
|
||||
inline void setTargetPosition(int32_t pos) {
|
||||
fasDisableInterrupts();
|
||||
move_value = pos;
|
||||
move_absolute = true;
|
||||
keep_running = false;
|
||||
keep_running_count_up = true;
|
||||
any_change = true;
|
||||
apply = true;
|
||||
fasEnableInterrupts();
|
||||
}
|
||||
inline void setTargetRelativePosition(int32_t move) {
|
||||
fasDisableInterrupts();
|
||||
if (any_change && !move_absolute) {
|
||||
move_value += move;
|
||||
} else {
|
||||
move_value = move;
|
||||
}
|
||||
move_absolute = false;
|
||||
keep_running = false;
|
||||
keep_running_count_up = true;
|
||||
any_change = true;
|
||||
apply = true;
|
||||
fasEnableInterrupts();
|
||||
}
|
||||
inline void setCubicAccelerationSteps(uint32_t s_cubic_steps) {
|
||||
if (s_h != s_cubic_steps) {
|
||||
fasDisableInterrupts();
|
||||
s_h = s_cubic_steps;
|
||||
recalc_ramp_steps = true;
|
||||
any_change = true;
|
||||
fasEnableInterrupts();
|
||||
}
|
||||
}
|
||||
inline void setSpeedInTicks(uint32_t min_step_ticks) {
|
||||
if (!valid_speed || (min_travel_ticks != min_step_ticks)) {
|
||||
fasDisableInterrupts();
|
||||
min_travel_ticks = min_step_ticks;
|
||||
valid_speed = true;
|
||||
any_change = true;
|
||||
fasEnableInterrupts();
|
||||
}
|
||||
}
|
||||
inline void setAcceleration(int32_t accel) {
|
||||
pmf_logarithmic new_pmfl_accel = pmfl_from((uint32_t)accel);
|
||||
if (!valid_acceleration || (pmfl_accel != new_pmfl_accel)) {
|
||||
fasDisableInterrupts();
|
||||
valid_acceleration = true;
|
||||
any_change = true;
|
||||
recalc_ramp_steps = true;
|
||||
pmfl_accel = new_pmfl_accel;
|
||||
fasEnableInterrupts();
|
||||
}
|
||||
}
|
||||
inline void setJumpStart(uint32_t jump_step) { s_jump = jump_step; }
|
||||
inline int8_t checkValidConfig() const {
|
||||
if (!valid_speed) {
|
||||
return MOVE_ERR_SPEED_IS_UNDEFINED;
|
||||
}
|
||||
if (!valid_acceleration) {
|
||||
return MOVE_ERR_ACCELERATION_IS_UNDEFINED;
|
||||
}
|
||||
return MOVE_OK;
|
||||
}
|
||||
};
|
||||
|
||||
struct ramp_config_s {
|
||||
struct ramp_parameters_s parameters;
|
||||
|
||||
// These three variables are derived
|
||||
uint32_t max_ramp_up_steps;
|
||||
pmf_logarithmic pmfl_ticks_h;
|
||||
pmf_logarithmic cubic;
|
||||
|
||||
void init() { parameters.init(); }
|
||||
inline void update() {
|
||||
if (parameters.s_h > 0) {
|
||||
pmf_logarithmic pmfl_s_h = pmfl_from(parameters.s_h);
|
||||
// 1/cubic = sqrt(3/2 * a) / s_h^(1/6) / TICKS_PER_S
|
||||
// = sqrt(3/2 * a / s_h^(1/3)) / TICKS_PER_S
|
||||
// cubic = TICKS_PER_S / sqrt(s_h^(1/3) / (3/2 * a))
|
||||
cubic = pmfl_multiply(PMF_CONST_3_DIV_2, parameters.pmfl_accel);
|
||||
cubic = pmfl_sqrt(pmfl_divide(pmfl_pow_div_3(pmfl_s_h), cubic));
|
||||
cubic = pmfl_multiply(PMF_TICKS_PER_S, cubic);
|
||||
|
||||
// calculate_ticks(s_h)
|
||||
pmfl_ticks_h = pmfl_divide(cubic, pmfl_pow_2_div_3(pmfl_s_h));
|
||||
} else {
|
||||
pmfl_ticks_h = PMF_CONST_MAX;
|
||||
}
|
||||
max_ramp_up_steps = calculate_ramp_steps(parameters.min_travel_ticks);
|
||||
if (max_ramp_up_steps == 0) {
|
||||
max_ramp_up_steps = 1;
|
||||
}
|
||||
#ifdef TEST
|
||||
printf("MAX_RAMP_UP_STEPS=%d from %d ticks\n", max_ramp_up_steps,
|
||||
parameters.min_travel_ticks);
|
||||
#endif
|
||||
}
|
||||
|
||||
uint32_t calculate_ticks(uint32_t steps) const {
|
||||
// s = 1/2 * a * t^2
|
||||
// 2*a*s = (a*t)^2 = v^2 = (TICKS_PER_S/ticks)^2
|
||||
// ticks = TICKS_PER_S / sqrt(2*a*s)
|
||||
// ticks = TICKS_PER_S/sqrt(2) / sqrt(a*s)
|
||||
if (steps >= parameters.s_h) {
|
||||
steps -= (parameters.s_h + 2) >> 2;
|
||||
pmf_logarithmic pmfl_steps = pmfl_from(steps);
|
||||
pmf_logarithmic pmfl_steps_mul_accel =
|
||||
pmfl_multiply(pmfl_steps, parameters.pmfl_accel);
|
||||
pmf_logarithmic pmfl_sqrt_steps_mul_accel =
|
||||
pmfl_sqrt(pmfl_steps_mul_accel);
|
||||
pmf_logarithmic pmfl_res =
|
||||
pmfl_divide(PMF_TICKS_PER_S_DIV_SQRT_OF_2, pmfl_sqrt_steps_mul_accel);
|
||||
uint32_t res = pmfl_to_u32(pmfl_res);
|
||||
return res;
|
||||
}
|
||||
// ticks = cubic / s^(2/3)
|
||||
pmf_logarithmic pmfl_steps = pmfl_from(steps);
|
||||
pmf_logarithmic pmfl_res = pmfl_divide(cubic, pmfl_pow_2_div_3(pmfl_steps));
|
||||
uint32_t res = pmfl_to_u32(pmfl_res);
|
||||
return res;
|
||||
}
|
||||
uint32_t calculate_ramp_steps(uint32_t ticks) const {
|
||||
// pmfl is in range -64..<64 due to shift by 1
|
||||
// pmfl_ticks is in range 0..<32
|
||||
// pmfl_accel is in range 0..<32
|
||||
// PMF_ACCEL_FACTOR is approx. 47 for 16 Mticks/s
|
||||
// pmfl_ticks squared is in range 0..<64
|
||||
pmf_logarithmic pmfl_ticks = pmfl_from(ticks);
|
||||
if (pmfl_ticks <= pmfl_ticks_h) {
|
||||
pmf_logarithmic pmfl_inv_accel2 =
|
||||
pmfl_divide(PMF_ACCEL_FACTOR, parameters.pmfl_accel);
|
||||
uint32_t steps =
|
||||
pmfl_to_u32(pmfl_divide(pmfl_inv_accel2, pmfl_square(pmfl_ticks)));
|
||||
steps += (parameters.s_h + 2) >> 2;
|
||||
return steps;
|
||||
}
|
||||
// s = (cubic/ticks)^(3/2)
|
||||
pmf_logarithmic pmfl_res = pmfl_divide(cubic, pmfl_ticks);
|
||||
pmfl_res = pmfl_pow_3_div_2(pmfl_res);
|
||||
uint32_t steps = pmfl_to_u32(pmfl_res);
|
||||
return steps;
|
||||
}
|
||||
};
|
||||
#endif
|
||||
529
src/RampConstAcceleration.cpp
Normal file
529
src/RampConstAcceleration.cpp
Normal file
@@ -0,0 +1,529 @@
|
||||
#include <cstdint>
|
||||
|
||||
#include "FastAccelStepper.h"
|
||||
#include "StepperISR.h"
|
||||
|
||||
#include "RampConstAcceleration.h"
|
||||
#include "fas_arch/common.h"
|
||||
|
||||
#ifdef SUPPORT_PMF_TIMER_FREQ_VARIABLES
|
||||
static pmf_logarithmic pmfl_timer_freq;
|
||||
static pmf_logarithmic pmfl_timer_freq_div_sqrt_of_2;
|
||||
static pmf_logarithmic pmfl_timer_freq_square_div_2;
|
||||
#endif
|
||||
|
||||
void init_ramp_module() {
|
||||
#ifdef SUPPORT_PMF_TIMER_FREQ_VARIABLES
|
||||
pmfl_timer_freq = pmfl_from((uint32_t)TICKS_PER_S);
|
||||
pmfl_timer_freq_div_sqrt_of_2 =
|
||||
pmfl_shr(pmfl_multiply(pmfl_timer_freq, pmfl_timer_freq));
|
||||
pmfl_timer_freq_square_div_2 = pmfl_shr(pmfl_square(pmfl_timer_freq));
|
||||
#endif
|
||||
}
|
||||
|
||||
//*************************************************************************************************
|
||||
|
||||
// #define TRACE
|
||||
#ifdef TRACE
|
||||
#define TRACE_OUTPUT(x) Serial.print(x)
|
||||
#else
|
||||
#define TRACE_OUTPUT(x)
|
||||
#endif
|
||||
|
||||
#ifdef TEST
|
||||
void print_ramp_state(uint8_t this_state) {
|
||||
switch (this_state & RAMP_DIRECTION_MASK) {
|
||||
case RAMP_DIRECTION_COUNT_UP:
|
||||
printf("+");
|
||||
break;
|
||||
case RAMP_DIRECTION_COUNT_DOWN:
|
||||
printf("-");
|
||||
break;
|
||||
}
|
||||
switch (this_state & RAMP_STATE_MASK) {
|
||||
case RAMP_STATE_COAST:
|
||||
printf("COAST");
|
||||
break;
|
||||
case RAMP_STATE_ACCELERATE:
|
||||
printf("ACC");
|
||||
break;
|
||||
case RAMP_STATE_DECELERATE:
|
||||
printf("DEC");
|
||||
break;
|
||||
case RAMP_STATE_REVERSE:
|
||||
printf("REVERSE");
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
//*************************************************************************************************
|
||||
void _getNextCommand(const struct ramp_ro_s *ramp, const struct ramp_rw_s *rw,
|
||||
const struct queue_end_s *queue_end,
|
||||
NextCommand *command) {
|
||||
{
|
||||
// If there is a pause from last step, then just output a pause
|
||||
uint32_t pause_ticks = rw->pause_ticks_left;
|
||||
if (pause_ticks > 0) {
|
||||
if (pause_ticks > 65535) {
|
||||
pause_ticks >>= 1;
|
||||
pause_ticks = fas_min(pause_ticks, 65535);
|
||||
}
|
||||
command->command.ticks = pause_ticks;
|
||||
command->command.steps = 0;
|
||||
command->command.count_up = queue_end->count_up;
|
||||
command->rw = *rw;
|
||||
command->rw.pause_ticks_left -= pause_ticks;
|
||||
#ifdef TEST
|
||||
printf("add command pause ticks = %d remaining pause = %d\n",
|
||||
pause_ticks, command->rw.pause_ticks_left);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
bool count_up = queue_end->count_up;
|
||||
|
||||
// check state for acceleration/deceleration or deceleration to stop
|
||||
uint8_t this_state;
|
||||
uint32_t remaining_steps;
|
||||
bool need_count_up;
|
||||
if (ramp->config.parameters.keep_running) {
|
||||
need_count_up = ramp->config.parameters.keep_running_count_up;
|
||||
remaining_steps = 0xfffffff;
|
||||
} else {
|
||||
// this can overflow, which is legal
|
||||
int32_t delta = ramp->target_pos - queue_end->pos;
|
||||
|
||||
if (delta == 0) {
|
||||
// this case can happen on overshoot. So reverse current direction
|
||||
need_count_up = !count_up;
|
||||
} else {
|
||||
need_count_up = delta > 0;
|
||||
}
|
||||
remaining_steps = fas_abs(delta);
|
||||
}
|
||||
|
||||
#ifdef TRACE
|
||||
if (remaining_steps == performed_ramp_up_steps) {
|
||||
Serial.print('=');
|
||||
} else if (remaining_steps > performed_ramp_up_steps) {
|
||||
uint32_t dx = remaining_steps - performed_ramp_up_steps;
|
||||
if (dx < 10) {
|
||||
char ch = '0' + dx;
|
||||
Serial.print(ch);
|
||||
Serial.print('x');
|
||||
}
|
||||
} else {
|
||||
uint32_t dx = performed_ramp_up_steps - remaining_steps;
|
||||
if (dx < 10) {
|
||||
char ch = '0' + dx;
|
||||
Serial.print(ch);
|
||||
Serial.print('y');
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
// If not moving, then use requested direction
|
||||
uint32_t performed_ramp_up_steps = rw->performed_ramp_up_steps;
|
||||
if (performed_ramp_up_steps == 0) {
|
||||
count_up = need_count_up;
|
||||
}
|
||||
|
||||
if ((remaining_steps == 0) && (performed_ramp_up_steps <= 1)) {
|
||||
command->command.ticks = 0;
|
||||
command->rw.pause_ticks_left = 0;
|
||||
command->rw.ramp_state = RAMP_STATE_IDLE;
|
||||
command->rw.curr_ticks = TICKS_FOR_STOPPED_MOTOR;
|
||||
#ifdef TEST
|
||||
puts("ramp complete");
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
||||
uint32_t curr_ticks = rw->curr_ticks;
|
||||
|
||||
// Forward planning of 2ms or more on slow speed.
|
||||
uint16_t planning_steps;
|
||||
if (curr_ticks < TICKS_PER_S / 1000) {
|
||||
uint16_t ps = TICKS_PER_S / 500;
|
||||
ps /= (uint16_t)curr_ticks;
|
||||
planning_steps = ps;
|
||||
} else {
|
||||
planning_steps = 1;
|
||||
}
|
||||
uint16_t orig_planning_steps = planning_steps;
|
||||
|
||||
// In case of force stop just run down the ramp
|
||||
if (count_up != need_count_up) {
|
||||
// On direction change, do reversing
|
||||
this_state = RAMP_STATE_REVERSE;
|
||||
remaining_steps = performed_ramp_up_steps;
|
||||
} else {
|
||||
// If come here, then direction is same as current movement
|
||||
if (remaining_steps == performed_ramp_up_steps) {
|
||||
this_state = RAMP_STATE_DECELERATE;
|
||||
// remaining_steps = performed_ramp_up_steps;
|
||||
} else if (remaining_steps < performed_ramp_up_steps) {
|
||||
// We will overshoot
|
||||
TRACE_OUTPUT('O');
|
||||
this_state = RAMP_STATE_REVERSE;
|
||||
remaining_steps = performed_ramp_up_steps;
|
||||
} else if (ramp->config.parameters.min_travel_ticks < rw->curr_ticks) {
|
||||
this_state = RAMP_STATE_ACCELERATE;
|
||||
if (rw->curr_ticks < 2 * MIN_CMD_TICKS) {
|
||||
// special consideration needed, that invalid commands are not generated
|
||||
//
|
||||
// possible coast steps is divided by 4: 1 part acc, 2 part coast, 1
|
||||
// part dec
|
||||
uint32_t possible_coast_steps =
|
||||
(remaining_steps - performed_ramp_up_steps) >> 2;
|
||||
if (possible_coast_steps > 0) {
|
||||
// curr_ticks is not necessarily correct due to speed increase
|
||||
uint32_t coast_time = possible_coast_steps * rw->curr_ticks;
|
||||
if (coast_time < 2 * MIN_CMD_TICKS) {
|
||||
TRACE_OUTPUT('l');
|
||||
this_state = RAMP_STATE_COAST;
|
||||
#ifdef TEST
|
||||
printf("high speed coast %d %d\n", possible_coast_steps,
|
||||
remaining_steps - performed_ramp_up_steps);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
if (planning_steps > remaining_steps - performed_ramp_up_steps) {
|
||||
this_state = RAMP_STATE_DECELERATE;
|
||||
}
|
||||
} else if (remaining_steps - performed_ramp_up_steps <
|
||||
2 * planning_steps) {
|
||||
if (curr_ticks != TICKS_FOR_STOPPED_MOTOR) {
|
||||
this_state = RAMP_STATE_COAST;
|
||||
planning_steps = remaining_steps - performed_ramp_up_steps;
|
||||
}
|
||||
}
|
||||
} else if (ramp->config.parameters.min_travel_ticks > rw->curr_ticks) {
|
||||
TRACE_OUTPUT('d');
|
||||
this_state = RAMP_STATE_DECELERATE;
|
||||
if (performed_ramp_up_steps <= planning_steps) {
|
||||
if (performed_ramp_up_steps > 0) {
|
||||
planning_steps = performed_ramp_up_steps;
|
||||
} else {
|
||||
planning_steps = 1;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
TRACE_OUTPUT('c');
|
||||
this_state = RAMP_STATE_COAST;
|
||||
uint32_t possible_coast_steps = remaining_steps - performed_ramp_up_steps;
|
||||
if (possible_coast_steps < 2 * planning_steps) {
|
||||
planning_steps = possible_coast_steps;
|
||||
if (curr_ticks < MIN_CMD_TICKS) {
|
||||
uint32_t cmd_ticks = curr_ticks * planning_steps;
|
||||
if (cmd_ticks < MIN_CMD_TICKS) {
|
||||
this_state = RAMP_STATE_DECELERATE;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (remaining_steps == 0) { // This implies performed_ramp_up_steps == 0
|
||||
command->command.ticks = 0;
|
||||
command->rw.pause_ticks_left = 0;
|
||||
command->rw.ramp_state = RAMP_STATE_IDLE;
|
||||
command->rw.curr_ticks = TICKS_FOR_STOPPED_MOTOR;
|
||||
#ifdef TEST
|
||||
puts("ramp complete");
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
||||
// Guarantee here:
|
||||
// remaining_steps > 0
|
||||
// remaining_steps >= performed_ramp_up_steps
|
||||
// remaining_steps > performed_ramp_up_steps, in COAST
|
||||
// performed_ramp_up_steps can be 0
|
||||
// planning_steps >= 1
|
||||
#ifdef TEST
|
||||
assert(remaining_steps > 0);
|
||||
assert(remaining_steps >= performed_ramp_up_steps);
|
||||
assert((remaining_steps > performed_ramp_up_steps) ||
|
||||
(this_state != RAMP_STATE_COAST));
|
||||
assert(planning_steps > 0);
|
||||
#endif
|
||||
|
||||
#ifdef TEST
|
||||
printf("prus=%d planning_steps=%d remaining_steps=%d force_stop=%d\n",
|
||||
performed_ramp_up_steps, planning_steps, remaining_steps,
|
||||
ramp->force_stop);
|
||||
#endif
|
||||
uint32_t d_ticks_new;
|
||||
{
|
||||
if (this_state & RAMP_STATE_ACCELERATING_FLAG) {
|
||||
TRACE_OUTPUT('A');
|
||||
// do not overshoot ramp down start
|
||||
//
|
||||
// seems to be not necessary, as consideration already done above
|
||||
uint32_t dec_steps = remaining_steps - performed_ramp_up_steps;
|
||||
if (dec_steps < 512) {
|
||||
// Only allow half, cause the steps accelerating need to decelerate, too
|
||||
auto dec_steps_u16 = (uint16_t)dec_steps;
|
||||
dec_steps_u16 /= 2;
|
||||
// Perhaps it would be better to coast instead
|
||||
// consideration has been done above already
|
||||
if (dec_steps_u16 < orig_planning_steps) {
|
||||
planning_steps = dec_steps_u16;
|
||||
if (planning_steps == 0) {
|
||||
planning_steps = 1;
|
||||
}
|
||||
#ifdef TEST
|
||||
printf("Change planning_steps=%u\n", planning_steps);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t rs = performed_ramp_up_steps + planning_steps;
|
||||
d_ticks_new = ramp->config.calculate_ticks(rs);
|
||||
#ifdef TEST
|
||||
printf("Calculate d_ticks_new=%u from ramp steps=%u\n", d_ticks_new, rs);
|
||||
#endif
|
||||
|
||||
// if acceleration is very high, then d_ticks_new can be lower than
|
||||
// min_travel_ticks
|
||||
if (d_ticks_new < ramp->config.parameters.min_travel_ticks) {
|
||||
d_ticks_new = ramp->config.parameters.min_travel_ticks;
|
||||
}
|
||||
} else if (this_state & RAMP_STATE_DECELERATING_FLAG) {
|
||||
TRACE_OUTPUT('D');
|
||||
if (performed_ramp_up_steps == 1) {
|
||||
d_ticks_new = ramp->config.parameters.min_travel_ticks;
|
||||
#ifdef TEST
|
||||
printf("Set d_ticks_new=%u to min_travel_ticks\n", d_ticks_new);
|
||||
#endif
|
||||
} else {
|
||||
uint32_t rs;
|
||||
if (performed_ramp_up_steps <= planning_steps) {
|
||||
rs = planning_steps;
|
||||
} else {
|
||||
rs = performed_ramp_up_steps - planning_steps;
|
||||
}
|
||||
d_ticks_new = ramp->config.calculate_ticks(rs);
|
||||
#ifdef TEST
|
||||
printf("Calculate d_ticks_new=%d from ramp steps=%d for deceleration\n",
|
||||
d_ticks_new, rs);
|
||||
#endif
|
||||
// If the ramp generator cannot decelerate by going down the ramp,
|
||||
// then we need to clip the new d_ticks to the min travel ticks
|
||||
// This is for issue #150
|
||||
uint32_t min_travel_ticks = ramp->config.parameters.min_travel_ticks;
|
||||
if ((rs == 1) && (min_travel_ticks > d_ticks_new)) {
|
||||
d_ticks_new = min_travel_ticks;
|
||||
#ifdef TEST
|
||||
printf("Clip d_ticks_new=%d for deceleration\n", d_ticks_new);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
} else {
|
||||
TRACE_OUTPUT('C');
|
||||
d_ticks_new = rw->curr_ticks;
|
||||
// do not overshoot ramp down start
|
||||
uint32_t coast_steps = remaining_steps - performed_ramp_up_steps;
|
||||
if (coast_steps < 256) {
|
||||
uint16_t coast_steps_u16 = coast_steps;
|
||||
if (coast_steps_u16 < 2 * orig_planning_steps) {
|
||||
planning_steps = coast_steps_u16;
|
||||
}
|
||||
}
|
||||
#ifdef TEST
|
||||
printf("planning steps=%d remaining steps=%d\n", planning_steps,
|
||||
remaining_steps);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
// The above plannings_steps evaluation uses curr_ticks,
|
||||
// but new_ticks can be lower and so the command time not sufficient
|
||||
if (d_ticks_new < MIN_CMD_TICKS) {
|
||||
uint32_t cmd_ticks = d_ticks_new * planning_steps;
|
||||
if (cmd_ticks < MIN_CMD_TICKS) {
|
||||
// using planning_steps and d_ticks_new would create invalid commands
|
||||
|
||||
uint16_t steps = MIN_CMD_TICKS + d_ticks_new - 1;
|
||||
steps /= ((uint16_t)d_ticks_new);
|
||||
#ifdef TEST
|
||||
printf("new steps=%d d_ticks_new=%d\n", steps, d_ticks_new);
|
||||
#endif
|
||||
if (steps >= remaining_steps) {
|
||||
#ifdef TEST
|
||||
printf(
|
||||
"command time too low, with increased steps will reach ramp end\n");
|
||||
#endif
|
||||
planning_steps = remaining_steps;
|
||||
this_state = RAMP_STATE_DECELERATE;
|
||||
}
|
||||
|
||||
// if we are at ramp end, then reduce speed
|
||||
steps = fas_max(planning_steps, steps);
|
||||
if (2 * steps >= remaining_steps) {
|
||||
d_ticks_new = MIN_CMD_TICKS + remaining_steps - 1;
|
||||
d_ticks_new /= remaining_steps;
|
||||
planning_steps = remaining_steps;
|
||||
this_state = RAMP_STATE_DECELERATE;
|
||||
#ifdef TEST
|
||||
printf("command time too low, so reduce speed to %d ticks\n",
|
||||
d_ticks_new);
|
||||
#endif
|
||||
} else {
|
||||
#ifdef TEST
|
||||
printf("Increase planning steps %d => %d due to command time\n",
|
||||
planning_steps, steps);
|
||||
#endif
|
||||
planning_steps = steps;
|
||||
|
||||
// do we need to decelerate in order to not overshoot ?
|
||||
if (remaining_steps < performed_ramp_up_steps + planning_steps) {
|
||||
this_state = RAMP_STATE_DECELERATE;
|
||||
|
||||
// and now the speed is actually too high....
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// perform clipping with current ticks
|
||||
uint32_t next_ticks = d_ticks_new;
|
||||
if (curr_ticks != TICKS_FOR_STOPPED_MOTOR) {
|
||||
if (this_state & RAMP_STATE_ACCELERATING_FLAG) {
|
||||
next_ticks = fas_min(next_ticks, curr_ticks);
|
||||
} else if (this_state & RAMP_STATE_DECELERATING_FLAG) {
|
||||
// CLIPPING: avoid reduction unless curr_ticks indicates stopped motor
|
||||
// Issue #25: root cause is, that curr_ticks can be
|
||||
// TICKS_FOR_STOPPED_MOTOR for the case, that queue is emptied before
|
||||
// the next command is issued
|
||||
next_ticks = fas_max(next_ticks, curr_ticks);
|
||||
// if (this_state != RAMP_STATE_DECELERATE) {
|
||||
// next_ticks = fas_max(next_ticks, ramp->config.min_travel_ticks);
|
||||
// }
|
||||
}
|
||||
}
|
||||
#ifdef TEST
|
||||
assert(next_ticks > 0);
|
||||
#endif
|
||||
#ifdef TEST
|
||||
if (next_ticks != d_ticks_new) {
|
||||
printf(
|
||||
"Clipping result d_ticks_new=%d => next_ticks=%d with curr_ticks=%d "
|
||||
"state=%d\n",
|
||||
d_ticks_new, next_ticks, curr_ticks, this_state);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef TEST
|
||||
printf("planning steps=%d remaining steps=%d prus=%d\n", planning_steps,
|
||||
remaining_steps, performed_ramp_up_steps);
|
||||
#endif
|
||||
// Number of steps to execute with limitation to min 1 and max remaining steps
|
||||
uint16_t steps = planning_steps;
|
||||
steps = fas_min(steps, remaining_steps); // This could be problematic
|
||||
steps = fas_max(steps, 1);
|
||||
steps = fas_min(255, steps);
|
||||
|
||||
// Check if pauses need to be added. If yes, reduce next_ticks and calculate
|
||||
// pause_ticks_left
|
||||
uint32_t pause_ticks_left;
|
||||
if (next_ticks > 65535) {
|
||||
steps = 1;
|
||||
pause_ticks_left = next_ticks;
|
||||
next_ticks >>= 1;
|
||||
next_ticks = fas_min(next_ticks, 65535);
|
||||
pause_ticks_left -= next_ticks;
|
||||
} else {
|
||||
pause_ticks_left = 0;
|
||||
}
|
||||
|
||||
// determine performed_ramp_up_steps after command enqueued
|
||||
if (this_state & RAMP_STATE_ACCELERATING_FLAG) {
|
||||
performed_ramp_up_steps += steps;
|
||||
} else if (this_state & RAMP_STATE_DECELERATING_FLAG) {
|
||||
if (performed_ramp_up_steps < steps) {
|
||||
// This can occur with performed_ramp_up_steps = 0 and steps = 1
|
||||
#ifdef TEST
|
||||
printf("prus=%d steps=%d\n", performed_ramp_up_steps, steps);
|
||||
// assert((performed_ramp_up_steps == 0) && (steps == 1));
|
||||
#endif
|
||||
// based on above assumption actually obsolete
|
||||
performed_ramp_up_steps = 0;
|
||||
} else {
|
||||
uint32_t max_ramp_up_steps = ramp->config.max_ramp_up_steps;
|
||||
#ifdef TEST
|
||||
printf("prus=%d steps=%d max_prus=%d\n", performed_ramp_up_steps, steps,
|
||||
max_ramp_up_steps);
|
||||
#endif
|
||||
if (performed_ramp_up_steps > max_ramp_up_steps) {
|
||||
#ifdef TEST
|
||||
printf("reduce prus=%d by %d\n", performed_ramp_up_steps, steps);
|
||||
#endif
|
||||
performed_ramp_up_steps -= steps;
|
||||
} else if ((performed_ramp_up_steps >= max_ramp_up_steps) &&
|
||||
(max_ramp_up_steps + steps <= remaining_steps) &&
|
||||
(performed_ramp_up_steps - steps < max_ramp_up_steps)) {
|
||||
// Speed was too high. So we need to ensure to not overshoot
|
||||
// deceleration
|
||||
#ifdef TEST
|
||||
printf("clip prus=%d to %d\n", performed_ramp_up_steps,
|
||||
max_ramp_up_steps);
|
||||
#endif
|
||||
performed_ramp_up_steps = max_ramp_up_steps;
|
||||
next_ticks = ramp->config.parameters.min_travel_ticks;
|
||||
} else {
|
||||
if (remaining_steps > performed_ramp_up_steps) {
|
||||
if (remaining_steps - performed_ramp_up_steps < steps) {
|
||||
performed_ramp_up_steps = remaining_steps - steps;
|
||||
#ifdef TEST
|
||||
printf("set prus to remaining steps %d minus %d steps\n",
|
||||
remaining_steps, steps);
|
||||
#endif
|
||||
}
|
||||
} else {
|
||||
performed_ramp_up_steps -= steps;
|
||||
#ifdef TEST
|
||||
printf("reduce prus by %d steps\n", steps);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (count_up) {
|
||||
this_state |= RAMP_DIRECTION_COUNT_UP;
|
||||
} else {
|
||||
this_state |= RAMP_DIRECTION_COUNT_DOWN;
|
||||
}
|
||||
|
||||
command->command.ticks = next_ticks;
|
||||
command->command.steps = steps;
|
||||
command->command.count_up = count_up;
|
||||
|
||||
command->rw.ramp_state = this_state;
|
||||
command->rw.performed_ramp_up_steps = performed_ramp_up_steps;
|
||||
command->rw.pause_ticks_left = pause_ticks_left;
|
||||
command->rw.curr_ticks = pause_ticks_left + next_ticks;
|
||||
|
||||
#ifdef TEST
|
||||
printf(
|
||||
"pos@queue_end=%d remaining=%u prus=%u planning steps=%d "
|
||||
"last_ticks=%u travel_ticks=%u ",
|
||||
queue_end->pos, remaining_steps, performed_ramp_up_steps, planning_steps,
|
||||
rw->curr_ticks, ramp->config.parameters.min_travel_ticks);
|
||||
print_ramp_state(this_state);
|
||||
printf("\n");
|
||||
printf(
|
||||
"add command Steps=%u ticks=%u Target pos=%u "
|
||||
"Remaining steps=%u, planning_steps=%u, "
|
||||
"d_ticks_new=%u, pause_left=%u\n",
|
||||
steps, next_ticks, ramp->target_pos, remaining_steps, planning_steps,
|
||||
d_ticks_new, pause_ticks_left);
|
||||
if ((this_state & RAMP_STATE_MASK) == RAMP_STATE_ACCELERATE) {
|
||||
assert(pause_ticks_left + next_ticks >=
|
||||
ramp->config.parameters.min_travel_ticks);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
79
src/RampConstAcceleration.h
Normal file
79
src/RampConstAcceleration.h
Normal file
@@ -0,0 +1,79 @@
|
||||
#ifndef RAMP_CONST_ACCELERATION_H
|
||||
#define RAMP_CONST_ACCELERATION_H
|
||||
|
||||
#include "fas_arch/common.h"
|
||||
|
||||
struct ramp_ro_s {
|
||||
struct ramp_config_s config;
|
||||
uint32_t target_pos;
|
||||
bool force_stop : 1;
|
||||
bool force_immediate_stop : 1;
|
||||
bool incomplete_immediate_stop : 1;
|
||||
inline void init() {
|
||||
config.init();
|
||||
force_stop = false;
|
||||
force_immediate_stop = false;
|
||||
}
|
||||
inline int32_t targetPosition() { return target_pos; }
|
||||
inline void setTargetPosition(int32_t pos) { target_pos = pos; }
|
||||
inline void advanceTargetPositionWithinInterruptDisabledScope(int32_t delta) {
|
||||
target_pos += delta;
|
||||
}
|
||||
inline void immediateStop() { force_immediate_stop = true; }
|
||||
inline bool isImmediateStopInitiated() { return force_immediate_stop; }
|
||||
inline void clearImmediateStop() { force_immediate_stop = false; }
|
||||
inline void initiateStop() { force_stop = true; }
|
||||
inline bool isStopInitiated() { return force_stop; }
|
||||
inline void setKeepRunning() { config.parameters.keep_running = true; }
|
||||
inline bool isRunningContinuously() { return config.parameters.keep_running; }
|
||||
};
|
||||
|
||||
struct ramp_rw_s {
|
||||
volatile uint8_t ramp_state;
|
||||
// the speed is linked on both ramp slopes to this variable as per (if no
|
||||
// cubic ramp)
|
||||
// s = v²/2a => v = sqrt(2*a*s)
|
||||
uint32_t performed_ramp_up_steps;
|
||||
// Are the ticks stored of the last previous step, if pulse time requires
|
||||
// more than one command
|
||||
uint32_t pause_ticks_left;
|
||||
// Current ticks for ongoing step
|
||||
uint32_t curr_ticks;
|
||||
inline void stopRamp() {
|
||||
ramp_state = RAMP_STATE_IDLE; // this prevents fill_queue to be executed
|
||||
pause_ticks_left = 0;
|
||||
performed_ramp_up_steps = 0;
|
||||
curr_ticks = TICKS_FOR_STOPPED_MOTOR;
|
||||
#ifdef TEST
|
||||
printf("stopRamp() called\n");
|
||||
#endif
|
||||
}
|
||||
inline void init() { stopRamp(); }
|
||||
inline uint8_t rampState() {
|
||||
// reading one byte is atomic
|
||||
return ramp_state;
|
||||
}
|
||||
inline void startRampIfNotRunning(uint32_t s_jump) {
|
||||
#ifdef TEST
|
||||
printf("startRampIfNotRunning(%d) called\n", s_jump);
|
||||
#endif
|
||||
// called with interrupts disabled
|
||||
if (ramp_state == RAMP_STATE_IDLE) {
|
||||
curr_ticks = TICKS_FOR_STOPPED_MOTOR;
|
||||
// ramp_state value is significant to start the ramp generator.
|
||||
// so initialize curr_ticks before
|
||||
ramp_state = RAMP_STATE_ACCELERATE;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
class NextCommand {
|
||||
public:
|
||||
struct stepper_command_s command;
|
||||
struct ramp_rw_s rw; // new _rw, if command has been queued
|
||||
};
|
||||
|
||||
void init_ramp_module();
|
||||
void _getNextCommand(const struct ramp_ro_s *ramp, const struct ramp_rw_s *rw,
|
||||
const struct queue_end_s *queue_end, NextCommand *command);
|
||||
#endif
|
||||
274
src/RampGenerator.cpp
Normal file
274
src/RampGenerator.cpp
Normal file
@@ -0,0 +1,274 @@
|
||||
#include <cstdint>
|
||||
|
||||
#include "FastAccelStepper.h"
|
||||
#include "RampGenerator.h"
|
||||
#include "StepperISR.h"
|
||||
|
||||
// This define in order to not shoot myself.
|
||||
#ifndef TEST
|
||||
#define printf DO_NOT_USE_PRINTF
|
||||
#define puts DO_NOT_USE_PRINTF
|
||||
#endif
|
||||
|
||||
void RampGenerator::init() {
|
||||
_parameters.init();
|
||||
_ro.init();
|
||||
_rw.init();
|
||||
init_ramp_module();
|
||||
}
|
||||
int8_t RampGenerator::setAcceleration(int32_t accel) {
|
||||
if (accel <= 0) {
|
||||
return -1;
|
||||
}
|
||||
acceleration = (uint32_t)accel;
|
||||
_parameters.setAcceleration(accel);
|
||||
return 0;
|
||||
}
|
||||
void RampGenerator::applySpeedAcceleration() {
|
||||
if (!_ro.isImmediateStopInitiated()) {
|
||||
_parameters.applyParameters();
|
||||
}
|
||||
}
|
||||
int8_t RampGenerator::startRun(bool countUp) {
|
||||
uint8_t res = _parameters.checkValidConfig();
|
||||
if (res != MOVE_OK) {
|
||||
return res;
|
||||
}
|
||||
_ro.force_stop = false;
|
||||
_parameters.setRunning(countUp);
|
||||
_rw.startRampIfNotRunning(_parameters.s_jump);
|
||||
#ifdef DEBUG
|
||||
char buf[256];
|
||||
sprintf(buf, "Ramp data: curr_ticks = %lu travel_ticks = %lu\n",
|
||||
_rw.curr_ticks, _parameters.min_travel_ticks);
|
||||
Serial.println(buf);
|
||||
#endif
|
||||
return MOVE_OK;
|
||||
}
|
||||
|
||||
void RampGenerator::_startMove(bool position_changed) {
|
||||
_ro.force_stop = false;
|
||||
|
||||
if (position_changed) {
|
||||
// Only start the ramp generator, if the target position is different
|
||||
_rw.startRampIfNotRunning(_parameters.s_jump);
|
||||
}
|
||||
|
||||
#ifdef TEST
|
||||
printf("Ramp data: go to %s %d curr_ticks = %u travel_ticks = %u prus=%u\n",
|
||||
_parameters.move_absolute ? "ABS" : "REL", _parameters.move_value,
|
||||
_rw.curr_ticks, _ro.config.parameters.min_travel_ticks,
|
||||
_rw.performed_ramp_up_steps);
|
||||
#endif
|
||||
#ifdef DEBUG
|
||||
char buf[256];
|
||||
sprintf(buf,
|
||||
"Ramp data: go to = %s %ld curr_ticks = %lu travel_ticks = %lu "
|
||||
"prus=%lu\n",
|
||||
_parameters.move_absolute ? "ABS" : "REL", _parameters.move_value,
|
||||
_rw.curr_ticks, _ro.config.parameters.min_travel_ticks,
|
||||
_rw.performed_ramp_up_steps);
|
||||
Serial.println(buf);
|
||||
#endif
|
||||
}
|
||||
|
||||
int8_t RampGenerator::moveTo(int32_t position,
|
||||
const struct queue_end_s *queue_end) {
|
||||
uint8_t res = _parameters.checkValidConfig();
|
||||
if (res != MOVE_OK) {
|
||||
return res;
|
||||
}
|
||||
int32_t curr_target;
|
||||
if (isRampGeneratorActive() && !_ro.config.parameters.keep_running) {
|
||||
curr_target = _ro.target_pos;
|
||||
} else {
|
||||
curr_target = queue_end->pos;
|
||||
}
|
||||
inject_fill_interrupt(1);
|
||||
_parameters.setTargetPosition(position);
|
||||
_startMove(curr_target != position);
|
||||
inject_fill_interrupt(2);
|
||||
return MOVE_OK;
|
||||
}
|
||||
int8_t RampGenerator::move(int32_t move, const struct queue_end_s *queue_end) {
|
||||
uint8_t res = _parameters.checkValidConfig();
|
||||
if (res != MOVE_OK) {
|
||||
return res;
|
||||
}
|
||||
_parameters.setTargetRelativePosition(move);
|
||||
_startMove(move != 0);
|
||||
return MOVE_OK;
|
||||
}
|
||||
void RampGenerator::advanceTargetPosition(int32_t delta,
|
||||
const struct queue_end_s *queue) {
|
||||
// called with interrupts disabled
|
||||
_ro.target_pos += delta;
|
||||
}
|
||||
|
||||
void RampGenerator::afterCommandEnqueued(NextCommand *command) {
|
||||
#ifdef TEST
|
||||
printf(
|
||||
"after Command Enqueued: performed ramp up steps = %u, pause left = %u, "
|
||||
"curr_ticks = %u\n",
|
||||
command->rw.performed_ramp_up_steps, command->rw.pause_ticks_left,
|
||||
command->rw.curr_ticks);
|
||||
#endif
|
||||
_rw = command->rw;
|
||||
}
|
||||
void RampGenerator::getNextCommand(const struct queue_end_s *queue_end,
|
||||
NextCommand *command) {
|
||||
// we are running in higher priority than the application
|
||||
// so we can just read the config without disable interrupts
|
||||
// copy consistent ramp state
|
||||
bool was_keep_running = _ro.config.parameters.keep_running;
|
||||
if (_parameters.apply) {
|
||||
_ro.config.parameters = _parameters;
|
||||
_parameters.apply = false;
|
||||
_parameters.any_change = false;
|
||||
_parameters.move_value = 0;
|
||||
_parameters.move_absolute = false;
|
||||
_parameters.recalc_ramp_steps = false;
|
||||
_ro.config.update();
|
||||
// if new move command,then reset any immediate stop flag
|
||||
if (_ro.isImmediateStopInitiated()) {
|
||||
if (_ro.config.parameters.move_absolute) {
|
||||
if (_ro.target_pos != (uint32_t)_ro.config.parameters.move_value) {
|
||||
_ro.clearImmediateStop();
|
||||
}
|
||||
} else if (_ro.config.parameters.move_value != 0) {
|
||||
_ro.clearImmediateStop();
|
||||
}
|
||||
}
|
||||
#ifdef DEBUG
|
||||
Serial.print("new command: move=");
|
||||
if (_ro.config.parameters.keep_running) {
|
||||
Serial.print(_ro.config.parameters.keep_running_count_up ? "forward"
|
||||
: "backward");
|
||||
} else {
|
||||
Serial.print(_ro.config.parameters.move_absolute ? '@' : 'r');
|
||||
Serial.print(_ro.config.parameters.move_value);
|
||||
}
|
||||
if (_ro.config.parameters.valid_speed) {
|
||||
Serial.print(" v=");
|
||||
Serial.print(_ro.config.parameters.min_travel_ticks);
|
||||
}
|
||||
if (_ro.config.parameters.valid_acceleration) {
|
||||
Serial.print(" a=");
|
||||
Serial.print(pmfl_to_u32(_ro.config.parameters.pmfl_accel));
|
||||
}
|
||||
if (_ro.config.parameters.recalc_ramp_steps) {
|
||||
Serial.print(" recalc");
|
||||
}
|
||||
Serial.println();
|
||||
#endif
|
||||
}
|
||||
|
||||
fasDisableInterrupts();
|
||||
struct queue_end_s qe = *queue_end;
|
||||
fasEnableInterrupts();
|
||||
|
||||
uint32_t curr_ticks = _rw.curr_ticks;
|
||||
if (curr_ticks == TICKS_FOR_STOPPED_MOTOR) {
|
||||
// just started
|
||||
uint32_t s_jump = _ro.config.parameters.s_jump;
|
||||
if (s_jump != 0) {
|
||||
uint32_t ticks = _ro.config.calculate_ticks(s_jump);
|
||||
if (ticks < _ro.config.parameters.min_travel_ticks) {
|
||||
s_jump = _ro.config.calculate_ramp_steps(
|
||||
_ro.config.parameters.min_travel_ticks);
|
||||
}
|
||||
}
|
||||
_rw.performed_ramp_up_steps = s_jump;
|
||||
_ro.config.parameters.recalc_ramp_steps = false;
|
||||
}
|
||||
|
||||
// If the acceleration has changed, recalculate the ramp up/down steps,
|
||||
// which is the equivalent to the current speed.
|
||||
// Even if the acceleration value is constant, the calculated value
|
||||
// can deviate due to precision or clipping effect
|
||||
if (_ro.config.parameters.recalc_ramp_steps) {
|
||||
uint32_t performed_ramp_up_steps =
|
||||
_ro.config.calculate_ramp_steps(curr_ticks);
|
||||
#ifdef TEST
|
||||
printf("Recalculate performed_ramp_up_steps from %d to %d from %d ticks\n",
|
||||
_rw.performed_ramp_up_steps, performed_ramp_up_steps, curr_ticks);
|
||||
#endif
|
||||
#ifdef DEBUG
|
||||
char buf[100];
|
||||
sprintf(
|
||||
buf,
|
||||
"Recalculate performed_ramp_up_steps from %lu to %lu from %lu ticks\n",
|
||||
_rw.performed_ramp_up_steps, performed_ramp_up_steps, curr_ticks);
|
||||
Serial.print(buf);
|
||||
#endif
|
||||
_rw.performed_ramp_up_steps = performed_ramp_up_steps;
|
||||
}
|
||||
|
||||
if (_ro.force_stop) {
|
||||
_ro.config.parameters.keep_running = false;
|
||||
uint32_t target_pos = qe.pos;
|
||||
if (qe.count_up) {
|
||||
target_pos += _rw.performed_ramp_up_steps;
|
||||
} else {
|
||||
target_pos -= _rw.performed_ramp_up_steps;
|
||||
}
|
||||
#ifdef TEST
|
||||
printf("Force stop: adjust target position from %d to %d\n", _ro.target_pos,
|
||||
target_pos);
|
||||
#endif
|
||||
#ifdef DEBUG
|
||||
char buf[100];
|
||||
sprintf(buf, "Force stop: adjust target position from %ld to %ld\n",
|
||||
_ro.target_pos, target_pos);
|
||||
Serial.print(buf);
|
||||
#endif
|
||||
_ro.target_pos = target_pos;
|
||||
} else if (_ro.config.parameters.any_change) {
|
||||
// calculate new target position
|
||||
_ro.config.parameters.any_change = false;
|
||||
if (_ro.config.parameters.keep_running) {
|
||||
} else if (_ro.config.parameters.move_absolute) {
|
||||
_ro.target_pos = _ro.config.parameters.move_value;
|
||||
} else {
|
||||
uint32_t target_pos = _ro.target_pos;
|
||||
if (was_keep_running) {
|
||||
// target_pos is not valid for keep running
|
||||
target_pos = qe.pos;
|
||||
}
|
||||
_ro.target_pos = target_pos + _ro.config.parameters.move_value;
|
||||
}
|
||||
#ifdef DEBUG
|
||||
Serial.print("target pos=");
|
||||
Serial.println(_ro.target_pos);
|
||||
#endif
|
||||
}
|
||||
|
||||
// This line is the root cause for the failure in issue #280
|
||||
//_ro.force_stop = false;
|
||||
|
||||
// clear recalc flag
|
||||
_ro.config.parameters.recalc_ramp_steps = false;
|
||||
|
||||
if (_ro.isImmediateStopInitiated()) {
|
||||
// no more commands
|
||||
command->command.ticks = 0;
|
||||
_ro.clearImmediateStop();
|
||||
_ro.target_pos = qe.pos;
|
||||
command->rw.stopRamp();
|
||||
return;
|
||||
}
|
||||
_getNextCommand(&_ro, &_rw, &qe, command);
|
||||
}
|
||||
int32_t RampGenerator::getCurrentAcceleration() {
|
||||
switch (_rw.rampState() &
|
||||
(RAMP_STATE_ACCELERATING_FLAG | RAMP_STATE_DECELERATING_FLAG |
|
||||
RAMP_DIRECTION_MASK)) {
|
||||
case RAMP_STATE_ACCELERATING_FLAG | RAMP_DIRECTION_COUNT_UP:
|
||||
case RAMP_STATE_DECELERATING_FLAG | RAMP_DIRECTION_COUNT_DOWN:
|
||||
return acceleration;
|
||||
case RAMP_STATE_DECELERATING_FLAG | RAMP_DIRECTION_COUNT_UP:
|
||||
case RAMP_STATE_ACCELERATING_FLAG | RAMP_DIRECTION_COUNT_DOWN:
|
||||
return -acceleration;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
118
src/RampGenerator.h
Normal file
118
src/RampGenerator.h
Normal file
@@ -0,0 +1,118 @@
|
||||
#ifndef RAMP_GENERATOR_H
|
||||
#define RAMP_GENERATOR_H
|
||||
|
||||
#include "FastAccelStepper.h"
|
||||
#include "RampCalculator.h"
|
||||
#include "RampConstAcceleration.h"
|
||||
#include "fas_arch/common.h"
|
||||
|
||||
class FastAccelStepper;
|
||||
|
||||
#ifdef SUPPORT_PMF_TIMER_FREQ_VARIABLES
|
||||
extern pmf_logarithmic pmfl_timer_freq;
|
||||
extern pmf_logarithmic pmfl_timer_freq_div_sqrt_of_2;
|
||||
extern pmf_logarithmic pmfl_timer_freq_square_div_2;
|
||||
#endif
|
||||
|
||||
class RampGenerator {
|
||||
private:
|
||||
struct ramp_parameters_s _parameters;
|
||||
|
||||
// The ro variables are those, which are only read from _getNextCommand().
|
||||
// The rw variables are only read and written by _getNextCommand() and
|
||||
// commandEnqueued() Reading ro variables is safe in application
|
||||
struct ramp_ro_s _ro;
|
||||
struct ramp_rw_s _rw;
|
||||
|
||||
public:
|
||||
uint32_t acceleration;
|
||||
inline uint8_t rampState() { return _rw.rampState(); }
|
||||
void init();
|
||||
inline int32_t targetPosition() { return _ro.targetPosition(); }
|
||||
inline void setTargetPosition(int32_t pos) { _ro.setTargetPosition(pos); }
|
||||
void advanceTargetPosition(int32_t delta, const struct queue_end_s *queue);
|
||||
inline void setSpeedInTicks(uint32_t min_step_ticks) {
|
||||
_parameters.setSpeedInTicks(min_step_ticks);
|
||||
}
|
||||
inline uint32_t getSpeedInUs() {
|
||||
return _parameters.min_travel_ticks / (TICKS_PER_S / 1000000);
|
||||
}
|
||||
inline uint32_t getSpeedInTicks() { return _parameters.min_travel_ticks; }
|
||||
uint32_t divForMilliHz(uint32_t f) {
|
||||
uint32_t base = (uint32_t)250 * TICKS_PER_S;
|
||||
uint32_t res = base / f;
|
||||
base -= res * f;
|
||||
base <<= 2;
|
||||
res <<= 2;
|
||||
base += f / 2; // add rounding
|
||||
res += base / f;
|
||||
return res;
|
||||
}
|
||||
uint32_t divForHz(uint32_t f) {
|
||||
uint32_t base = TICKS_PER_S;
|
||||
base += f / 2; // add rounding
|
||||
uint32_t res = base / f;
|
||||
return res;
|
||||
}
|
||||
inline uint32_t getSpeedInMilliHz() {
|
||||
if (_parameters.min_travel_ticks == 0) {
|
||||
return 0;
|
||||
}
|
||||
return divForMilliHz(getSpeedInTicks());
|
||||
}
|
||||
int8_t setAcceleration(int32_t accel);
|
||||
inline uint32_t getAcceleration() { return acceleration; }
|
||||
inline void setLinearAcceleration(uint32_t linear_acceleration_steps) {
|
||||
_parameters.setCubicAccelerationSteps(linear_acceleration_steps);
|
||||
}
|
||||
inline void setJumpStart(uint32_t jump_step) {
|
||||
_parameters.setJumpStart(jump_step);
|
||||
}
|
||||
int32_t getCurrentAcceleration();
|
||||
inline bool hasValidConfig() {
|
||||
return _parameters.checkValidConfig() == MOVE_OK;
|
||||
}
|
||||
void applySpeedAcceleration();
|
||||
int8_t move(int32_t move, const struct queue_end_s *queue);
|
||||
int8_t moveTo(int32_t position, const struct queue_end_s *queue);
|
||||
int8_t startRun(bool countUp);
|
||||
inline void forceStop() { _ro.immediateStop(); }
|
||||
inline void initiateStop() { _ro.initiateStop(); }
|
||||
inline bool isStopping() {
|
||||
return _ro.isStopInitiated() && isRampGeneratorActive();
|
||||
}
|
||||
inline bool isRampGeneratorActive() { return rampState() != RAMP_STATE_IDLE; }
|
||||
inline uint32_t stepsToStop() {
|
||||
fasDisableInterrupts();
|
||||
uint32_t v = _rw.performed_ramp_up_steps;
|
||||
fasEnableInterrupts();
|
||||
return v;
|
||||
}
|
||||
inline void stopRamp() { _rw.stopRamp(); }
|
||||
inline void setKeepRunning() { _ro.setKeepRunning(); }
|
||||
inline bool isRunningContinuously() { return _ro.isRunningContinuously(); }
|
||||
void getNextCommand(const struct queue_end_s *queue_end,
|
||||
NextCommand *cmd_out);
|
||||
void afterCommandEnqueued(NextCommand *cmd_in);
|
||||
void getCurrentSpeedInTicks(struct actual_ticks_s *speed) {
|
||||
fasDisableInterrupts();
|
||||
speed->ticks = _rw.curr_ticks;
|
||||
uint8_t rs = _rw.rampState();
|
||||
fasEnableInterrupts();
|
||||
speed->count_up = ((rs & RAMP_DIRECTION_COUNT_UP) != 0);
|
||||
}
|
||||
uint32_t getCurrentPeriodInTicks() {
|
||||
fasDisableInterrupts();
|
||||
uint32_t ticks = _rw.curr_ticks;
|
||||
fasEnableInterrupts();
|
||||
return ticks;
|
||||
}
|
||||
inline uint32_t getCurrentPeriodInUs() {
|
||||
return TICKS_TO_US(getCurrentPeriodInTicks());
|
||||
}
|
||||
|
||||
private:
|
||||
void _startMove(bool position_changed);
|
||||
};
|
||||
|
||||
#endif
|
||||
349
src/StepperISR.cpp
Normal file
349
src/StepperISR.cpp
Normal file
@@ -0,0 +1,349 @@
|
||||
#include <stdint.h>
|
||||
|
||||
#include "StepperISR.h"
|
||||
|
||||
int8_t StepperQueue::addQueueEntry(const struct stepper_command_s* cmd,
|
||||
bool start) {
|
||||
// Just to check if, if the struct has the correct size
|
||||
// if (sizeof(entry) != 6 * QUEUE_LEN) {
|
||||
// return -1;
|
||||
//}
|
||||
if (!isReadyForCommands()) {
|
||||
return AQE_DEVICE_NOT_READY;
|
||||
}
|
||||
if (cmd == NULL) {
|
||||
if (start && !isRunning()) {
|
||||
if (next_write_idx == read_idx) {
|
||||
return AQE_ERROR_EMPTY_QUEUE_TO_START;
|
||||
}
|
||||
startQueue();
|
||||
}
|
||||
return AQE_OK;
|
||||
}
|
||||
if (isQueueFull()) {
|
||||
return AQE_QUEUE_FULL;
|
||||
}
|
||||
uint16_t period = cmd->ticks;
|
||||
uint8_t steps = cmd->steps;
|
||||
// generation discrepancy: pc vs target
|
||||
// after Command Enqueued: performed ramp up steps = 1, pause left = 0,
|
||||
// curr_ticks = 11320 after Command Enqueued: performed ramp up steps = 3,
|
||||
// pause left = 0, curr_ticks = 6536 after Command Enqueued: performed ramp up
|
||||
// steps = 7, pause left = 0, curr_ticks = 4276 after Command Enqueued:
|
||||
// performed ramp up steps = 14, pause left = 0, curr_ticks = 3024 after
|
||||
// Command Enqueued: performed ramp up steps = 24, pause left = 0, curr_ticks
|
||||
// = 2312 after Command Enqueued: performed ramp up steps = 17, pause left =
|
||||
// 0, curr_ticks = 3412 after Command Enqueued: performed ramp up steps = 8,
|
||||
// pause left = 0, curr_ticks = 4004 after Command Enqueued: performed ramp up
|
||||
// steps = 1, pause left = 0, curr_ticks = 11320
|
||||
//
|
||||
// esp32 mcpwm and rmt:
|
||||
// 0/268435455us/4294967295:1:11320X:2:6536X:4:4276X:7:3024X:10:2312X:13:3412X:9:4004X:7:11320X:1:11320X
|
||||
//
|
||||
//
|
||||
// Failed 10steps:
|
||||
// :0:40000X:0:40000X:1:56608X:0:56608X:1:40032X:0:40032X:1:65344X:1:56608X:1:50624X:1:56608X:1:65344X:1:40032X:0:40032X:1:56608X:0:56608X:1:56608X:0:56608X
|
||||
//
|
||||
// Failed seq_07
|
||||
// :0:40000X:0:40000X:1:56608X:0:56608X:1:40032X:0:40032X:1:65344X:1:56608X:1:50624X
|
||||
// :1:46208X:1:42784X:1:40032X:1:37728X:1:35808X:1:34112X:1:32672X:1:31376X:1:30256X:1:29216X:1:28304X:1:27440X:1:26672X:1:25968X:1:25312X:1:24688X:1:24128X:1:23616X:1:23104X:1:22640X:1:22192X:1:21776X:1:21392X:1:21024X:1:20656X:1:20320X:1:20016X:1:19696X:1:19408X:1:19152X:1:18864X:1:18608X:1:18352X:1:18144X:1:17904X:1:17680X:1:17472X:1:17280X:1:17056X:1:16880X:1:16704X:1:16512X:1:16336X:1:16160X:1:16016X:1:15840X:2:15544X:2:15272X:2:14984X:2:14744X:2:14488X:2:14256X:2:14040X:2:13840X:2:13632X
|
||||
// :2:13432X:2:13248X:2:13072X:2:12896X:2:12736X:2:12584X:2:12432X:2:12280X:2:12128X:2:12000X:2:11872X:2:11744X:2:11616X:2:11488X:2:11384X:2:11264X:2:11152X:2:11048X:2:10944X:2:10840X:2:10736X:2:10656X:3:10512X:3:10384X:3:10248X:3:10120X:3:10008X:3:9888X:3:9784X:3:9680X:3:9576X:3:9472X:3:9368X:3:9280X:3:9176X:3:9096X:3:9008X:3:8928X:3:8840X:3:8760X:3:8688X:3:8600X:3:8528X:3:8464X:3:8392X:3:8328X:3:8256X:3:8192X:3:8124X:3:8060X:3:8008X:3:7944X:4:7864X:4:7792X:4:7720X:4:7648X:4:7584X:4:7512X:4:7452X:4:7384X:4:7324X
|
||||
// :4:7264X:4:7204X:4:7148X:4:7088X:4:7040X:4:6984X:4:6928X
|
||||
|
||||
// #define TRACE
|
||||
#ifdef TRACE
|
||||
Serial.print(':');
|
||||
Serial.print(start ? "START" : "PUSH");
|
||||
Serial.print(':');
|
||||
Serial.print(cmd->count_up ? 'U' : 'D');
|
||||
Serial.print(':');
|
||||
Serial.print(steps);
|
||||
Serial.print(':');
|
||||
Serial.print(period);
|
||||
Serial.print('X');
|
||||
#endif
|
||||
|
||||
uint32_t command_rate_ticks = period;
|
||||
if (steps > 1) {
|
||||
command_rate_ticks *= steps;
|
||||
}
|
||||
if (command_rate_ticks < MIN_CMD_TICKS) {
|
||||
return AQE_ERROR_TICKS_TOO_LOW;
|
||||
}
|
||||
|
||||
uint8_t wp = next_write_idx;
|
||||
struct queue_entry* e = &entry[wp & QUEUE_LEN_MASK];
|
||||
bool dir = (cmd->count_up == dirHighCountsUp);
|
||||
bool toggle_dir = false;
|
||||
#if defined(SUPPORT_EXTERNAL_DIRECTION_PIN)
|
||||
bool repeat_entry = false;
|
||||
#endif
|
||||
if (dirPin != PIN_UNDEFINED) {
|
||||
if ((isQueueEmpty() && !isRunning()) &&
|
||||
((dirPin & PIN_EXTERNAL_FLAG) == 0)) {
|
||||
// set the dirPin here. Necessary with shared direction pins
|
||||
digitalWrite(dirPin, dir);
|
||||
#ifdef ARDUINO_ARCH_SAM
|
||||
delayMicroseconds(30); // Make sure the driver has enough time to see
|
||||
// the dir pin change
|
||||
#endif
|
||||
queue_end.dir = dir;
|
||||
} else {
|
||||
toggle_dir = (dir != queue_end.dir);
|
||||
#if defined(SUPPORT_EXTERNAL_DIRECTION_PIN)
|
||||
if (toggle_dir && (dirPin & PIN_EXTERNAL_FLAG)) {
|
||||
repeat_entry = toggle_dir;
|
||||
toggle_dir = false;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
e->steps = steps;
|
||||
#if defined(SUPPORT_EXTERNAL_DIRECTION_PIN)
|
||||
e->repeat_entry = repeat_entry;
|
||||
e->dirPinState = dir;
|
||||
#endif
|
||||
e->toggle_dir = toggle_dir;
|
||||
e->countUp = cmd->count_up ? 1 : 0;
|
||||
e->moreThanOneStep = steps > 1 ? 1 : 0;
|
||||
e->hasSteps = steps > 0 ? 1 : 0;
|
||||
e->ticks = period;
|
||||
struct queue_end_s next_queue_end = queue_end;
|
||||
#if defined(SUPPORT_QUEUE_ENTRY_START_POS_U16)
|
||||
e->start_pos_last16 = (uint32_t)next_queue_end.pos & 0xffff;
|
||||
#endif
|
||||
next_queue_end.pos = next_queue_end.pos + (cmd->count_up ? steps : -steps);
|
||||
#if defined(SUPPORT_QUEUE_ENTRY_END_POS_U16)
|
||||
e->end_pos_last16 = (uint32_t)next_queue_end.pos & 0xffff;
|
||||
#endif
|
||||
next_queue_end.dir = dir;
|
||||
next_queue_end.count_up = cmd->count_up;
|
||||
|
||||
// Advance write pointer
|
||||
fasDisableInterrupts();
|
||||
if (!ignore_commands) {
|
||||
if (isReadyForCommands()) {
|
||||
next_write_idx = next_write_idx + 1;
|
||||
queue_end = next_queue_end;
|
||||
} else {
|
||||
fasEnableInterrupts();
|
||||
return AQE_DEVICE_NOT_READY;
|
||||
}
|
||||
}
|
||||
fasEnableInterrupts();
|
||||
|
||||
if (!isRunning() && start) {
|
||||
// stepper is not yet running and start is requested
|
||||
#ifdef TRACE
|
||||
Serial.print('S');
|
||||
#endif
|
||||
startQueue();
|
||||
}
|
||||
#ifdef TRACE
|
||||
else {
|
||||
// WHY IS start 0 in seq_01c
|
||||
Serial.print(isRunning() ? 'R' : 'T');
|
||||
Serial.print(start ? '1' : '0');
|
||||
Serial.println('N');
|
||||
}
|
||||
#endif
|
||||
return AQE_OK;
|
||||
}
|
||||
|
||||
int32_t StepperQueue::getCurrentPosition() {
|
||||
fasDisableInterrupts();
|
||||
uint32_t pos = (uint32_t)queue_end.pos;
|
||||
uint8_t rp = read_idx;
|
||||
bool is_empty = (rp == next_write_idx);
|
||||
struct queue_entry* e = &entry[rp & QUEUE_LEN_MASK];
|
||||
#if defined(SUPPORT_QUEUE_ENTRY_END_POS_U16)
|
||||
uint16_t pos_last16 = e->end_pos_last16;
|
||||
#endif
|
||||
#if defined(SUPPORT_QUEUE_ENTRY_START_POS_U16)
|
||||
uint16_t pos_last16 = e->start_pos_last16;
|
||||
#endif
|
||||
uint8_t steps = e->steps;
|
||||
#if defined(SUPPORT_ESP32)
|
||||
// pulse counter should go max up to 255 with perhaps few pulses overrun, so
|
||||
// this conversion is safe
|
||||
int16_t done_p = (int16_t)_getPerformedPulses();
|
||||
#endif
|
||||
fasEnableInterrupts();
|
||||
#if defined(SUPPORT_ESP32)
|
||||
if (done_p == 0) {
|
||||
// fix for possible race condition described in issue #68
|
||||
fasDisableInterrupts();
|
||||
rp = read_idx;
|
||||
is_empty = (rp == next_write_idx);
|
||||
e = &entry[rp & QUEUE_LEN_MASK];
|
||||
pos_last16 = e->start_pos_last16;
|
||||
steps = e->steps;
|
||||
done_p = (int16_t)_getPerformedPulses();
|
||||
fasEnableInterrupts();
|
||||
}
|
||||
#endif
|
||||
if (!is_empty) {
|
||||
int16_t adjust = 0;
|
||||
|
||||
uint16_t pos16 = pos & 0xffff;
|
||||
uint8_t transition = ((pos16 >> 12) & 0x0c) | (pos_last16 >> 14);
|
||||
switch (transition) {
|
||||
case 0: // 00 00
|
||||
case 5: // 01 01
|
||||
case 10: // 10 10
|
||||
case 15: // 11 11
|
||||
break;
|
||||
case 1: // 00 01
|
||||
case 6: // 01 10
|
||||
case 11: // 10 11
|
||||
case 12: // 11 00
|
||||
pos += 0x4000;
|
||||
break;
|
||||
case 4: // 01 00
|
||||
case 9: // 10 01
|
||||
case 14: // 11 10
|
||||
case 3: // 00 11
|
||||
pos -= 0x4000;
|
||||
break;
|
||||
case 2: // 00 10
|
||||
case 7: // 01 11
|
||||
case 8: // 10 00
|
||||
case 13: // 11 01
|
||||
break; // TODO: ERROR
|
||||
}
|
||||
pos = (int32_t)((pos & 0xffff0000) | pos_last16);
|
||||
|
||||
if (steps != 0) {
|
||||
if (e->countUp) {
|
||||
#if defined(SUPPORT_QUEUE_ENTRY_END_POS_U16)
|
||||
adjust = -steps;
|
||||
#endif
|
||||
#if defined(SUPPORT_QUEUE_ENTRY_START_POS_U16)
|
||||
adjust = done_p;
|
||||
#endif
|
||||
} else {
|
||||
#if defined(SUPPORT_QUEUE_ENTRY_END_POS_U16)
|
||||
adjust = steps;
|
||||
#endif
|
||||
#if defined(SUPPORT_QUEUE_ENTRY_START_POS_U16)
|
||||
adjust = -done_p;
|
||||
#endif
|
||||
}
|
||||
pos += adjust;
|
||||
}
|
||||
}
|
||||
return pos;
|
||||
}
|
||||
|
||||
uint32_t StepperQueue::ticksInQueue() {
|
||||
fasDisableInterrupts();
|
||||
uint8_t rp = read_idx;
|
||||
uint8_t wp = next_write_idx;
|
||||
fasEnableInterrupts();
|
||||
if (wp == rp) {
|
||||
return 0;
|
||||
}
|
||||
uint32_t ticks = 0;
|
||||
rp++; // ignore currently processed entry
|
||||
while (wp != rp) {
|
||||
struct queue_entry* e = &entry[rp++ & QUEUE_LEN_MASK];
|
||||
ticks += e->ticks;
|
||||
uint8_t steps = e->steps;
|
||||
if (steps > 1) {
|
||||
uint32_t tmp = e->ticks;
|
||||
tmp *= steps - 1;
|
||||
ticks += tmp;
|
||||
}
|
||||
}
|
||||
return ticks;
|
||||
}
|
||||
|
||||
bool StepperQueue::hasTicksInQueue(uint32_t min_ticks) {
|
||||
fasDisableInterrupts();
|
||||
uint8_t rp = read_idx;
|
||||
uint8_t wp = next_write_idx;
|
||||
fasEnableInterrupts();
|
||||
if (wp == rp) {
|
||||
return false;
|
||||
}
|
||||
rp++; // ignore currently processed entry
|
||||
while (wp != rp) {
|
||||
struct queue_entry* e = &entry[rp & QUEUE_LEN_MASK];
|
||||
uint32_t tmp = e->ticks;
|
||||
uint8_t steps = fas_max(e->steps, (uint8_t)1);
|
||||
tmp *= steps;
|
||||
if (tmp >= min_ticks) {
|
||||
return true;
|
||||
}
|
||||
min_ticks -= tmp;
|
||||
rp++;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool StepperQueue::getActualTicksWithDirection(struct actual_ticks_s* speed) {
|
||||
// Retrieve current step rate from the current command.
|
||||
// This is valid only, if the command describes more than one step,
|
||||
// or if the next command contains one step, too.
|
||||
fasDisableInterrupts();
|
||||
uint8_t rp = read_idx;
|
||||
uint8_t wp = next_write_idx;
|
||||
fasEnableInterrupts();
|
||||
if (wp == rp) {
|
||||
speed->ticks = 0;
|
||||
return true;
|
||||
}
|
||||
struct queue_entry* e = &entry[rp & QUEUE_LEN_MASK];
|
||||
if (e->hasSteps) {
|
||||
speed->count_up = e->countUp;
|
||||
speed->ticks = e->ticks;
|
||||
if (e->moreThanOneStep) {
|
||||
return true;
|
||||
}
|
||||
if (wp != ++rp) {
|
||||
if (entry[rp & QUEUE_LEN_MASK].hasSteps) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void StepperQueue::_initVars() {
|
||||
dirPin = PIN_UNDEFINED;
|
||||
#ifndef TEST
|
||||
max_speed_in_ticks = TICKS_PER_S / 1000; // use a default value 1_000 steps/s
|
||||
#else
|
||||
max_speed_in_ticks =
|
||||
TICKS_PER_S / 50000; // use a default value 50_000 steps/s
|
||||
#endif
|
||||
ignore_commands = false;
|
||||
read_idx = 0;
|
||||
next_write_idx = 0;
|
||||
queue_end.dir = true;
|
||||
queue_end.count_up = true;
|
||||
queue_end.pos = 0;
|
||||
dirHighCountsUp = true;
|
||||
#if defined(ARDUINO_ARCH_AVR)
|
||||
_isRunning = false;
|
||||
_noMoreCommands = false;
|
||||
#endif
|
||||
#if defined(SUPPORT_ESP32)
|
||||
_isRunning = false;
|
||||
_nextCommandIsPrepared = false;
|
||||
#endif
|
||||
#if defined(SUPPORT_ESP32_RMT)
|
||||
_rmtStopped = true;
|
||||
#endif
|
||||
#if defined(ARDUINO_ARCH_SAM)
|
||||
_hasISRactive = false;
|
||||
// we cannot clear the PWM interrupt when switching to a pause, but we'll
|
||||
// get a double interrupt if we do nothing. So this tells us that on a
|
||||
// transition from a pulse to a pause to skip the next interrupt.
|
||||
_pauseCommanded = false;
|
||||
timePWMInterruptEnabled = 0;
|
||||
#endif
|
||||
#if defined(TEST)
|
||||
_isRunning = false;
|
||||
#endif
|
||||
}
|
||||
202
src/StepperISR.h
Normal file
202
src/StepperISR.h
Normal file
@@ -0,0 +1,202 @@
|
||||
#include <stdint.h>
|
||||
|
||||
#include "FastAccelStepper.h"
|
||||
#include "fas_arch/common.h"
|
||||
|
||||
// Here are the global variables to interface with the interrupts
|
||||
|
||||
// These variables control the stepper timing behaviour
|
||||
#define QUEUE_LEN_MASK (QUEUE_LEN - 1)
|
||||
|
||||
struct queue_entry {
|
||||
uint8_t steps; // if 0, then the command only adds a delay
|
||||
uint8_t toggle_dir : 1;
|
||||
uint8_t countUp : 1;
|
||||
uint8_t moreThanOneStep : 1;
|
||||
uint8_t hasSteps : 1;
|
||||
#if defined(SUPPORT_EXTERNAL_DIRECTION_PIN)
|
||||
// if repeat_entry==1, then this entry shall be repeated.
|
||||
// This mechanism only works for pauses (steps == 0)
|
||||
// Used for external direction pin
|
||||
uint8_t repeat_entry : 1;
|
||||
uint8_t dirPinState : 1;
|
||||
#endif
|
||||
uint16_t ticks;
|
||||
#if defined(SUPPORT_QUEUE_ENTRY_END_POS_U16)
|
||||
uint16_t end_pos_last16;
|
||||
#endif
|
||||
#if defined(SUPPORT_QUEUE_ENTRY_START_POS_U16)
|
||||
uint16_t start_pos_last16;
|
||||
#endif
|
||||
};
|
||||
|
||||
class StepperQueue {
|
||||
public:
|
||||
struct queue_entry entry[QUEUE_LEN];
|
||||
|
||||
// In case of forceStopAndNewPosition() the adding of commands has to be
|
||||
// temporarily suspended
|
||||
volatile bool ignore_commands;
|
||||
volatile uint8_t read_idx; // ISR stops if readptr == next_writeptr
|
||||
volatile uint8_t next_write_idx;
|
||||
bool dirHighCountsUp;
|
||||
uint8_t dirPin;
|
||||
|
||||
// a word to isRunning():
|
||||
// if isRunning() is false, then the _QUEUE_ is not running.
|
||||
//
|
||||
// For esp32 this does NOT mean, that the HW is finished.
|
||||
// The timer is still counting down to zero until it stops at 0.
|
||||
// But there will be no interrupt to process another command.
|
||||
// So the queue requires startQueue() again
|
||||
//
|
||||
// Due to the rmt version of esp32, there has been the needed to
|
||||
// provide information, that device is not yet ready for new commands.
|
||||
// This has been called isReadyForCommands().
|
||||
//
|
||||
|
||||
#if defined(SUPPORT_ESP32)
|
||||
volatile bool _isRunning;
|
||||
bool _nextCommandIsPrepared;
|
||||
inline bool isRunning() { return _isRunning; }
|
||||
bool isReadyForCommands() const;
|
||||
bool use_rmt;
|
||||
uint8_t _step_pin;
|
||||
uint16_t _getPerformedPulses() const;
|
||||
#endif
|
||||
#ifdef SUPPORT_ESP32_MCPWM_PCNT
|
||||
const void* driver_data;
|
||||
#endif
|
||||
#ifdef SUPPORT_ESP32_RMT
|
||||
RMT_CHANNEL_T channel;
|
||||
bool _rmtStopped;
|
||||
#if ESP_IDF_VERSION_MAJOR == 4
|
||||
bool bufferContainsSteps[2];
|
||||
#else
|
||||
bool lastChunkContainsSteps;
|
||||
rmt_encoder_handle_t _tx_encoder;
|
||||
#endif
|
||||
#endif
|
||||
#if defined(SUPPORT_DIR_PIN_MASK)
|
||||
volatile SUPPORT_DIR_PIN_MASK* _dirPinPort;
|
||||
SUPPORT_DIR_PIN_MASK _dirPinMask;
|
||||
#endif
|
||||
#if defined(SUPPORT_DIR_TOGGLE_PIN_MASK)
|
||||
volatile SUPPORT_DIR_TOGGLE_PIN_MASK* _dirTogglePinPort;
|
||||
SUPPORT_DIR_TOGGLE_PIN_MASK _dirTogglePinMask;
|
||||
#endif
|
||||
#if defined(SUPPORT_AVR)
|
||||
volatile bool _noMoreCommands;
|
||||
volatile bool _isRunning;
|
||||
inline bool isRunning() { return _isRunning; }
|
||||
inline bool isReadyForCommands() { return true; }
|
||||
enum channels channel;
|
||||
#endif
|
||||
#if defined(SUPPORT_SAM)
|
||||
uint8_t _step_pin;
|
||||
uint8_t _queue_num;
|
||||
void* driver_data;
|
||||
volatile bool _hasISRactive;
|
||||
bool isRunning();
|
||||
bool _connected;
|
||||
inline bool isReadyForCommands() { return true; }
|
||||
volatile bool _pauseCommanded;
|
||||
volatile uint32_t timePWMInterruptEnabled;
|
||||
#endif
|
||||
#if defined(TEST)
|
||||
volatile bool _isRunning;
|
||||
inline bool isReadyForCommands() { return true; }
|
||||
inline bool isRunning() { return _isRunning; }
|
||||
#endif
|
||||
|
||||
struct queue_end_s queue_end;
|
||||
uint16_t max_speed_in_ticks;
|
||||
|
||||
void init(uint8_t queue_num, uint8_t step_pin);
|
||||
inline uint8_t queueEntries() {
|
||||
fasDisableInterrupts();
|
||||
uint8_t rp = read_idx;
|
||||
uint8_t wp = next_write_idx;
|
||||
fasEnableInterrupts();
|
||||
inject_fill_interrupt(0);
|
||||
return (uint8_t)(wp - rp);
|
||||
}
|
||||
inline bool isQueueFull() { return queueEntries() == QUEUE_LEN; }
|
||||
inline bool isQueueEmpty() { return queueEntries() == 0; }
|
||||
#if defined(SUPPORT_EXTERNAL_DIRECTION_PIN)
|
||||
inline bool isOnRepeatingEntry() {
|
||||
return entry[read_idx & QUEUE_LEN_MASK].repeat_entry == 1;
|
||||
}
|
||||
inline uint8_t dirPinState() {
|
||||
return entry[read_idx & QUEUE_LEN_MASK].dirPinState;
|
||||
}
|
||||
inline void clearRepeatingFlag() {
|
||||
entry[read_idx & QUEUE_LEN_MASK].repeat_entry = 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
int8_t addQueueEntry(const struct stepper_command_s* cmd, bool start);
|
||||
int32_t getCurrentPosition();
|
||||
uint32_t ticksInQueue();
|
||||
bool hasTicksInQueue(uint32_t min_ticks);
|
||||
bool getActualTicksWithDirection(struct actual_ticks_s* speed);
|
||||
|
||||
inline uint16_t getMaxSpeedInTicks() { return max_speed_in_ticks; }
|
||||
|
||||
// startQueue is always called
|
||||
void startQueue();
|
||||
void forceStop();
|
||||
void _initVars();
|
||||
void connect();
|
||||
void disconnect();
|
||||
|
||||
#ifdef SUPPORT_ESP32_MCPWM_PCNT
|
||||
bool isReadyForCommands_mcpwm_pcnt();
|
||||
void init_mcpwm_pcnt(uint8_t channel_num, uint8_t step_pin);
|
||||
void startQueue_mcpwm_pcnt();
|
||||
void forceStop_mcpwm_pcnt();
|
||||
uint16_t _getPerformedPulses_mcpwm_pcnt();
|
||||
void connect_mcpwm_pcnt();
|
||||
void disconnect_mcpwm_pcnt();
|
||||
#endif
|
||||
#ifdef SUPPORT_ESP32_RMT
|
||||
bool isReadyForCommands_rmt() const;
|
||||
void init_rmt(uint8_t channel_num, uint8_t step_pin);
|
||||
void startQueue_rmt();
|
||||
#if ESP_IDF_VERSION_MAJOR == 4
|
||||
void stop_rmt(bool both);
|
||||
#else
|
||||
bool _channel_enabled;
|
||||
#endif
|
||||
void forceStop_rmt();
|
||||
static uint16_t _getPerformedPulses_rmt();
|
||||
void connect_rmt();
|
||||
void disconnect_rmt();
|
||||
#endif
|
||||
void setDirPin(uint8_t dir_pin, bool _dirHighCountsUp) {
|
||||
dirPin = dir_pin;
|
||||
dirHighCountsUp = _dirHighCountsUp;
|
||||
#if defined(SUPPORT_DIR_PIN_MASK)
|
||||
if ((dir_pin != PIN_UNDEFINED) && ((dir_pin & PIN_EXTERNAL_FLAG) == 0)) {
|
||||
_dirPinPort = portOutputRegister(digitalPinToPort(dir_pin));
|
||||
_dirPinMask = digitalPinToBitMask(dir_pin);
|
||||
}
|
||||
#endif
|
||||
#if defined(SUPPORT_DIR_TOGGLE_PIN_MASK)
|
||||
if ((dir_pin != PIN_UNDEFINED) && ((dir_pin & PIN_EXTERNAL_FLAG) == 0)) {
|
||||
_dirTogglePinPort = portInputRegister(digitalPinToPort(dir_pin));
|
||||
_dirTogglePinMask = digitalPinToBitMask(dir_pin);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
#if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING == 1
|
||||
void setAbsoluteSpeedLimit(uint16_t ticks) { max_speed_in_ticks = ticks; }
|
||||
#endif
|
||||
void adjustSpeedToStepperCount(uint8_t steppers);
|
||||
static bool isValidStepPin(uint8_t step_pin);
|
||||
static int8_t queueNumForStepPin(uint8_t step_pin);
|
||||
};
|
||||
|
||||
extern StepperQueue fas_queue[NUM_QUEUES];
|
||||
|
||||
void fas_init_engine(FastAccelStepperEngine* engine, uint8_t cpu_core);
|
||||
359
src/StepperISR_avr.cpp
Normal file
359
src/StepperISR_avr.cpp
Normal file
@@ -0,0 +1,359 @@
|
||||
#if defined(ARDUINO_ARCH_AVR)
|
||||
#include "AVRStepperPins.h"
|
||||
#include "StepperISR.h"
|
||||
|
||||
// T is the timer module number 0,1,2,3...
|
||||
// X is the Channel name A or B
|
||||
//
|
||||
// BV1 Bv0
|
||||
// Output one is 1 1
|
||||
// Output zero is 1 0
|
||||
// Toggle is 0 1
|
||||
// Disconnect is 0 0
|
||||
//
|
||||
#define Stepper_Zero(T, X) \
|
||||
TCCR##T##A = (TCCR##T##A | _BV(COM##T##X##1)) & ~_BV(COM##T##X##0)
|
||||
// Force compare of Stepper_Toggle appears to be broken in simavr
|
||||
// In other words, use of Stepper_Toggle yields errors in simavr, for which root
|
||||
// cause is unclear
|
||||
#ifdef DISABLE
|
||||
#define Stepper_Toggle(T, X) \
|
||||
TCCR##T##A = (TCCR##T##A | _BV(COM##T##X##0)) & ~_BV(COM##T##X##1)
|
||||
#endif
|
||||
#define Stepper_One(T, X) TCCR##T##A |= _BV(COM##T##X##1) | _BV(COM##T##X##0)
|
||||
#define Stepper_Disconnect(T, X) \
|
||||
TCCR##T##A &= ~(_BV(COM##T##X##1) | _BV(COM##T##X##0))
|
||||
#define Stepper_IsOne(T, X) \
|
||||
((TCCR##T##A & (_BV(COM##T##X##0) | _BV(COM##T##X##1))) == \
|
||||
(_BV(COM##T##X##0) | _BV(COM##T##X##1)))
|
||||
#define Stepper_IsDisconnected(T, X) \
|
||||
((TCCR##T##A & (_BV(COM##T##X##0) | _BV(COM##T##X##1))) == 0)
|
||||
#define Stepper_IsOneIfOutput(T, X) ((TCCR##T##A & _BV(COM##T##X##0)) != 0)
|
||||
#define Stepper_ToggleDirection(CHANNEL) \
|
||||
*fas_queue_##CHANNEL._dirTogglePinPort = fas_queue_##CHANNEL._dirTogglePinMask
|
||||
#define PREPARE_DIRECTION_PIN(CHANNEL) \
|
||||
if (e->toggle_dir) { \
|
||||
Stepper_ToggleDirection(CHANNEL); \
|
||||
}
|
||||
|
||||
#ifdef SIMAVR_TIME_MEASUREMENT
|
||||
#define prepareISRtimeMeasurement() DDRB |= 0x18
|
||||
#define enterStepperISR() PORTB |= 0x08
|
||||
#define exitStepperISR() PORTB ^= 0x08
|
||||
#define enterFillQueueISR() PORTB |= 0x10
|
||||
#define exitFillQueueISR() PORTB ^= 0x10
|
||||
#elif defined(SIMAVR_TIME_MEASUREMENT_QUEUE)
|
||||
#define prepareISRtimeMeasurement() DDRB |= 0x10
|
||||
#define enterStepperISR() \
|
||||
{}
|
||||
#define exitStepperISR() \
|
||||
{}
|
||||
#define enterFillQueueISR() PORTB |= 0x10
|
||||
#define exitFillQueueISR() PORTB ^= 0x10
|
||||
#else
|
||||
#define prepareISRtimeMeasurement() \
|
||||
{}
|
||||
#define enterStepperISR() \
|
||||
{}
|
||||
#define exitStepperISR() \
|
||||
{}
|
||||
#define enterFillQueueISR() \
|
||||
{}
|
||||
#define exitFillQueueISR() \
|
||||
{}
|
||||
#endif
|
||||
|
||||
#ifdef SUPPORT_EXTERNAL_DIRECTION_PIN
|
||||
#define TEST_NOT_REPEATING_ENTRY (e->repeat_entry == 0)
|
||||
#else
|
||||
#define TEST_NOT_REPEATING_ENTRY (0 == 0)
|
||||
#endif
|
||||
|
||||
#define ForceCompare(T, X) TCCR##T##C = _BV(FOC##T##X)
|
||||
#define DisableCompareInterrupt(T, X) TIMSK##T &= ~_BV(OCIE##T##X)
|
||||
#define EnableCompareInterrupt(T, X) TIMSK##T |= _BV(OCIE##T##X)
|
||||
#define ClearInterruptFlag(T, X) TIFR##T = _BV(OCF##T##X)
|
||||
#define SetTimerCompareRelative(T, X, D) OCR##T##X = TCNT##T + D
|
||||
#define InterruptFlagIsSet(T, X) ((TIFR##T & _BV(OCF##T##X)) != 0)
|
||||
|
||||
#define ConfigureTimer(T) \
|
||||
{ \
|
||||
/* Set WGMn3:0 to all zero => Normal mode */ \
|
||||
TCCR##T##A &= ~(_BV(WGM##T##1) | _BV(WGM##T##0)); \
|
||||
TCCR##T##B &= ~(_BV(WGM##T##3) | _BV(WGM##T##2)); \
|
||||
/* Set prescaler to 1 */ \
|
||||
TCCR##T##B = \
|
||||
(TCCR##T##B & ~(_BV(CS##T##2) | _BV(CS##T##1) | _BV(CS##T##0))) | \
|
||||
_BV(CS##T##0); \
|
||||
}
|
||||
#define EnableOverflowInterrupt(T) TIMSK##T |= _BV(TOIE##T)
|
||||
#define DisableOverflowInterrupt(T) TIMSK##T &= ~_BV(TOIE##T)
|
||||
|
||||
// this is needed to give the background task isr access to engine
|
||||
static FastAccelStepperEngine* fas_engine = NULL;
|
||||
|
||||
// Here are the global variables to interface with the interrupts
|
||||
StepperQueue fas_queue[NUM_QUEUES];
|
||||
|
||||
#define AVR_INIT(T, CHANNEL) \
|
||||
{ \
|
||||
/* Disconnect stepper on next compare event */ \
|
||||
Stepper_Disconnect(T, CHANNEL); \
|
||||
/* disable compare A interrupt */ \
|
||||
DisableCompareInterrupt(T, CHANNEL); \
|
||||
/* force compare to ensure disconnect */ \
|
||||
ForceCompare(T, CHANNEL); \
|
||||
/* Initialize timer for correct time base */ \
|
||||
ConfigureTimer(T); \
|
||||
/* ensure cyclic interrupt is running */ \
|
||||
EnableOverflowInterrupt(T); \
|
||||
}
|
||||
void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) {
|
||||
prepareISRtimeMeasurement();
|
||||
_initVars();
|
||||
digitalWrite(step_pin, LOW);
|
||||
pinMode(step_pin, OUTPUT);
|
||||
if (step_pin == stepPinStepperA) {
|
||||
channel = channelA;
|
||||
AVR_INIT(FAS_TIMER_MODULE, A)
|
||||
}
|
||||
if (step_pin == stepPinStepperB) {
|
||||
channel = channelB;
|
||||
AVR_INIT(FAS_TIMER_MODULE, B)
|
||||
}
|
||||
#ifdef stepPinStepperC
|
||||
if (step_pin == stepPinStepperC) {
|
||||
channel = channelC;
|
||||
AVR_INIT(FAS_TIMER_MODULE, C)
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
// The interrupt is called on compare event, which eventually
|
||||
// generates a L->H transition. In any case, the current command's
|
||||
// wait time has still to be executed for the next command, if any.
|
||||
//
|
||||
// Remark: Interrupt Flag is automatically cleared on ISR execution
|
||||
//
|
||||
// If reaching here without further commands, then the queue is done
|
||||
#define AVR_STEPPER_ISR(T, CHANNEL) \
|
||||
ISR(TIMER##T##_COMP##CHANNEL##_vect) { \
|
||||
enterStepperISR(); \
|
||||
uint8_t rp = fas_queue_##CHANNEL.read_idx; \
|
||||
uint8_t wp = fas_queue_##CHANNEL.next_write_idx; \
|
||||
if (rp == wp) { \
|
||||
/* queue is empty => set to disconnect */ \
|
||||
/* disable compare interrupt */ \
|
||||
DisableCompareInterrupt(T, CHANNEL); \
|
||||
Stepper_Disconnect(T, CHANNEL); \
|
||||
/* force compare to ensure disconnect */ \
|
||||
ForceCompare(T, CHANNEL); \
|
||||
fas_queue_##CHANNEL._isRunning = false; \
|
||||
fas_queue_##CHANNEL._noMoreCommands = false; \
|
||||
exitStepperISR(); \
|
||||
return; \
|
||||
} \
|
||||
struct queue_entry* e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; \
|
||||
/* There is a risk, that this new compare time is delayed by one cycle */ \
|
||||
uint16_t ticks = e->ticks; \
|
||||
/* Set output to zero, this works in any case. In case of pause: no-op */ \
|
||||
Stepper_Zero(T, CHANNEL); \
|
||||
ForceCompare(T, CHANNEL); \
|
||||
if (e->steps > 1) { \
|
||||
/* perform another step with this queue entry */ \
|
||||
e->steps--; \
|
||||
Stepper_One(T, CHANNEL); \
|
||||
} else { \
|
||||
/* either pause command or no more steps */ \
|
||||
if (fas_queue_##CHANNEL._noMoreCommands) { \
|
||||
/* new command received after running out of commands */ \
|
||||
/* if this new command requires a step, then this step would be lost \
|
||||
*/ \
|
||||
fas_queue_##CHANNEL._noMoreCommands = false; \
|
||||
if (e->steps != 0) { \
|
||||
/* New command needs steps, so do it immediately */ \
|
||||
ticks = 10; \
|
||||
} \
|
||||
} else if (TEST_NOT_REPEATING_ENTRY) { \
|
||||
rp++; \
|
||||
fas_queue_##CHANNEL.read_idx = rp; \
|
||||
if (rp == wp) { \
|
||||
/* queue is empty, wait this command to complete, then disconnect */ \
|
||||
fas_queue_##CHANNEL._noMoreCommands = true; \
|
||||
OCR##T##CHANNEL += ticks; \
|
||||
exitStepperISR(); \
|
||||
return; \
|
||||
} \
|
||||
e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; \
|
||||
} \
|
||||
if (e->toggle_dir) { \
|
||||
Stepper_ToggleDirection(CHANNEL); \
|
||||
} \
|
||||
if (e->steps != 0) { \
|
||||
Stepper_One(T, CHANNEL); \
|
||||
} \
|
||||
} \
|
||||
OCR##T##CHANNEL += ticks; \
|
||||
exitStepperISR(); \
|
||||
}
|
||||
|
||||
#define AVR_STEPPER_ISR_GEN(T, CHANNEL) AVR_STEPPER_ISR(T, CHANNEL)
|
||||
AVR_STEPPER_ISR_GEN(FAS_TIMER_MODULE, A)
|
||||
AVR_STEPPER_ISR_GEN(FAS_TIMER_MODULE, B)
|
||||
#ifdef stepPinStepperC
|
||||
AVR_STEPPER_ISR_GEN(FAS_TIMER_MODULE, C)
|
||||
#endif
|
||||
|
||||
// this is for cyclic task
|
||||
#define AVR_CYCLIC_ISR(T) \
|
||||
ISR(TIMER##T##_OVF_vect) { \
|
||||
enterFillQueueISR(); \
|
||||
\
|
||||
/* disable OVF interrupt to avoid nesting */ \
|
||||
DisableOverflowInterrupt(T); \
|
||||
\
|
||||
/* enable interrupts for nesting */ \
|
||||
sei(); \
|
||||
\
|
||||
/* manage steppers */ \
|
||||
fas_engine->manageSteppers(); \
|
||||
\
|
||||
/* disable interrupts for exist ISR routine */ \
|
||||
cli(); \
|
||||
\
|
||||
/* enable OVF interrupt again */ \
|
||||
EnableOverflowInterrupt(T); \
|
||||
\
|
||||
exitFillQueueISR(); \
|
||||
}
|
||||
#define AVR_CYCLIC_ISR_GEN(T) AVR_CYCLIC_ISR(T)
|
||||
AVR_CYCLIC_ISR_GEN(FAS_TIMER_MODULE)
|
||||
|
||||
#define GET_ENTRY_PTR(T, CHANNEL) \
|
||||
rp = fas_queue_##CHANNEL.read_idx; \
|
||||
e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK];
|
||||
|
||||
#define AVR_START_QUEUE(T, CHANNEL) \
|
||||
/* ensure no compare event */ \
|
||||
SetTimerCompareRelative(T, CHANNEL, 32768); \
|
||||
/* set output one, if steps to be generated */ \
|
||||
if (e->steps != 0) { \
|
||||
Stepper_One(T, CHANNEL); \
|
||||
} else { \
|
||||
Stepper_Zero(T, CHANNEL); \
|
||||
} \
|
||||
/* clear interrupt flag */ \
|
||||
ClearInterruptFlag(T, CHANNEL); \
|
||||
/* enable compare interrupt */ \
|
||||
EnableCompareInterrupt(T, CHANNEL); \
|
||||
/* start */ \
|
||||
noInterrupts(); \
|
||||
SetTimerCompareRelative(T, CHANNEL, 10); \
|
||||
interrupts();
|
||||
|
||||
void StepperQueue::startQueue() {
|
||||
uint8_t rp;
|
||||
struct queue_entry* e;
|
||||
|
||||
_isRunning = true;
|
||||
switch (channel) {
|
||||
case channelA:
|
||||
GET_ENTRY_PTR(FAS_TIMER_MODULE, A)
|
||||
PREPARE_DIRECTION_PIN(A)
|
||||
AVR_START_QUEUE(FAS_TIMER_MODULE, A)
|
||||
break;
|
||||
case channelB:
|
||||
GET_ENTRY_PTR(FAS_TIMER_MODULE, B)
|
||||
PREPARE_DIRECTION_PIN(B)
|
||||
AVR_START_QUEUE(FAS_TIMER_MODULE, B)
|
||||
break;
|
||||
#ifdef stepPinStepperC
|
||||
case channelC:
|
||||
GET_ENTRY_PTR(FAS_TIMER_MODULE, C)
|
||||
PREPARE_DIRECTION_PIN(C)
|
||||
AVR_START_QUEUE(FAS_TIMER_MODULE, C)
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
#define FORCE_STOP(T, CHANNEL) \
|
||||
{ \
|
||||
/* disable compare interrupt */ \
|
||||
DisableCompareInterrupt(T, CHANNEL); \
|
||||
/* set to disconnect */ \
|
||||
Stepper_Disconnect(T, CHANNEL); \
|
||||
/* force compare to ensure disconnect */ \
|
||||
ForceCompare(T, CHANNEL); \
|
||||
}
|
||||
void StepperQueue::forceStop() {
|
||||
switch (channel) {
|
||||
case channelA:
|
||||
FORCE_STOP(FAS_TIMER_MODULE, A)
|
||||
break;
|
||||
case channelB:
|
||||
FORCE_STOP(FAS_TIMER_MODULE, B)
|
||||
break;
|
||||
#ifdef stepPinStepperC
|
||||
case channelC:
|
||||
FORCE_STOP(FAS_TIMER_MODULE, C)
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
_isRunning = false;
|
||||
|
||||
// empty the queue
|
||||
read_idx = next_write_idx;
|
||||
}
|
||||
void StepperQueue::connect() {}
|
||||
void StepperQueue::disconnect() {}
|
||||
bool StepperQueue::isValidStepPin(uint8_t step_pin) {
|
||||
if (step_pin == stepPinStepperA) {
|
||||
return true;
|
||||
}
|
||||
if (step_pin == stepPinStepperB) {
|
||||
return true;
|
||||
}
|
||||
#ifdef stepPinStepperC
|
||||
if (step_pin == stepPinStepperC) {
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
return false;
|
||||
}
|
||||
|
||||
int8_t StepperQueue::queueNumForStepPin(uint8_t step_pin) {
|
||||
if (step_pin == stepPinStepperA) {
|
||||
return 0;
|
||||
}
|
||||
if (step_pin == stepPinStepperB) {
|
||||
return 1;
|
||||
}
|
||||
#ifdef stepPinStepperC
|
||||
if (step_pin == stepPinStepperC) {
|
||||
return 2;
|
||||
}
|
||||
#endif
|
||||
return -1;
|
||||
}
|
||||
void StepperQueue::adjustSpeedToStepperCount(uint8_t steppers) {
|
||||
// commit 9577e9bfd4b9a6cf1ad830901c00c8b129a62aee fails
|
||||
// test_sd_04_timing_2560 as timer 3 reaches 40us.
|
||||
// This includes port set/clear for timer measurement.
|
||||
// So choose 20kHz
|
||||
//
|
||||
// check if Issue_152.ino, the interrupt need 14us.
|
||||
// So 70000 Steps/s is too high.
|
||||
if (steppers == 1) {
|
||||
max_speed_in_ticks = TICKS_PER_S / 50000;
|
||||
} else if (steppers == 2) {
|
||||
max_speed_in_ticks = 426;
|
||||
} else {
|
||||
max_speed_in_ticks = TICKS_PER_S / 20000;
|
||||
}
|
||||
}
|
||||
|
||||
void fas_init_engine(FastAccelStepperEngine* engine, uint8_t cpu_core) {
|
||||
fas_engine = engine;
|
||||
}
|
||||
#endif
|
||||
719
src/StepperISR_due.cpp
Normal file
719
src/StepperISR_due.cpp
Normal file
@@ -0,0 +1,719 @@
|
||||
#include "StepperISR.h"
|
||||
// #ifndef ARDUINO_ARCH_SAM
|
||||
// #define ARDUINO_ARCH_SAM
|
||||
// #endif
|
||||
// #define KEEP_SCORE
|
||||
#if defined(ARDUINO_ARCH_SAM)
|
||||
const bool enabled = false;
|
||||
uint32_t junk = 0;
|
||||
/*
|
||||
I went through a ton of iterations of trying to make this work sd desired.
|
||||
It seems quite simple, set the pulse period of the PWM generator, start it,
|
||||
stop it after e->ticks number of ticks. Except for that its never easy.
|
||||
|
||||
We start with how to detect that e->ticks has happened. I found that
|
||||
regaurdless of output mode, the PIO device is always connected and receiving
|
||||
inputs! This means I can use it to trigger on a pulse being sent! But this
|
||||
means I will get an interrupt for every single pulse. Thats not ideal, and I
|
||||
wasn't sure this would be able to operate fast enough in this mode. I
|
||||
actually did what I could to get into the PWM shutdown as fast as possible.
|
||||
However, it seems like it would still send one more pulse every time. It
|
||||
wouldn't register a full rise on my scope, and it probably....wouldn't have
|
||||
triggered a pulse on a stepper driver...but probably isn't good enough. No
|
||||
matter what I did, I wasn't fast enough. So I found that using the PIO to
|
||||
disconnect the output of the pulse generator was nearly instantaneous. So
|
||||
fast that to keep the interrupt on a rising edge, I need to delay a few
|
||||
microseconds to keep from chopping off the pulse! In fact, my driver says
|
||||
it needs 5 microsecond pulses. I'm currently sending 10 microsencond pulses.
|
||||
With a 5 microsecond delay before shutting off, the pulse is only 7
|
||||
microseconds in length! The performance of the code seems quite good! So
|
||||
not only is the overhead of the interrupts low enough to chop pulses off,
|
||||
its good enough to easily manage at least one stepper. (I need to make code
|
||||
more generic and make ISRs for the other ports and test with more).
|
||||
|
||||
The last "trick" that was necessary was a delay in starting the pulse generator
|
||||
after setting the direction pin. Currently my test just spins the motor 1
|
||||
revolution then spins it the other way. So every stop needs a delay. The
|
||||
delay is 100% necessary for setting the direction pin, so it cannot simply be
|
||||
removed!
|
||||
|
||||
With that, I've spun this motor to about 1600 RPM. No matter how slow the
|
||||
acceleration, it seems to stop somewhere just above that. So the code can do
|
||||
more than my nema23 test motor :)
|
||||
|
||||
Last "trick". I didn't think about the fact that at 21MHz with a 16 bit
|
||||
counter, the longest the system can delay is around 3ms. Fortunately, this was
|
||||
not lost on the original author. the hasSteps or steps==0 case is when you
|
||||
want to pause for a time (specified in the ticks member). To pause, I can
|
||||
disconnect the PWM peripheral, and generate PWM interrupts instead of using
|
||||
the PIO interrupts. It may seem like a good idea to just use the PWM
|
||||
interrupts. However, this arrangement actually eliminates a branch, and keeps
|
||||
pause code seprate from pulse code. On top of that, I get a true count of
|
||||
pulses output, rather than pulses attempted to be generated even if it was
|
||||
disconnected. Its actually more accurate than my scope, as if I am in
|
||||
process of disconnecting the PWM peripheral, I still get a PIO interrupt, but
|
||||
on the scope, I see only a small .8-.9V pulse. Its possible that pulse could
|
||||
be detected, though not likely. Better to eliminate it if possible (which it
|
||||
was!) The branch is eliminated by not needing to check if it was the cmpr
|
||||
register interrupt, or the period update interrupt. The PWM interrupt is
|
||||
always the period update, and the PIO is always the cmp. One branch isn't
|
||||
much, but its something!
|
||||
|
||||
|
||||
I think I'm going about this all wrong. I think I can set the AB reg to the PWM
|
||||
generator, but set the output to 0 and still get interrupts on the PIO when a
|
||||
pulse would have happened if we hadn't overridden the output to 0. Look into
|
||||
this...
|
||||
*/
|
||||
inline void disconnectPWMPeriphal(Pio* port, uint8_t pin, uint32_t channelMask);
|
||||
|
||||
typedef struct _PWMCHANNELMAPPING {
|
||||
uint8_t pin;
|
||||
uint32_t channel;
|
||||
Pio* port;
|
||||
uint32_t channelMask;
|
||||
} PWMCHANNELMAPPING;
|
||||
|
||||
#define NUM_PWM_CHANNELS 8
|
||||
uint8_t TimerChannel_Map[NUM_PWM_CHANNELS];
|
||||
|
||||
uint8_t numChannels = 0;
|
||||
|
||||
// this is needed to give the background task isr access to engine
|
||||
static FastAccelStepperEngine* fas_engine = NULL;
|
||||
|
||||
// Here are the global variables to interface with the interrupts
|
||||
#if defined(KEEP_SCORE) // If you want to count how many pulses we detect...
|
||||
volatile uint32_t totalPulsesDetected[NUM_QUEUES];
|
||||
volatile uint32_t totalSteps[NUM_QUEUES];
|
||||
inline void IncrementQueue(int queue) { totalPulsesDetected[queue]++; }
|
||||
inline void AddToTotalSteps(int queue, uint32_t steps) {
|
||||
totalSteps[queue] += steps;
|
||||
}
|
||||
#else
|
||||
#define IncrementQueue(Q)
|
||||
#define AddToTotalSteps(Q, N)
|
||||
#endif
|
||||
bool channelsUsed[8] = {false, false, false, false, false, false, false, false};
|
||||
StepperQueue fas_queue[NUM_QUEUES];
|
||||
PWMCHANNELMAPPING gChannelMap[NUM_QUEUES];
|
||||
|
||||
void TC5_Handler() {
|
||||
uint32_t SR0 = TC1->TC_CHANNEL[2].TC_SR;
|
||||
if (SR0 & TC_SR_CPCS) {
|
||||
// We're going to use TC5 as its not connected to any pins on the DUE to run
|
||||
// our engine... We only get in here on an RC compare, so if we're in here,
|
||||
// run manage steppers...
|
||||
fas_engine->manageSteppers();
|
||||
TC1->TC_CHANNEL[2].TC_CCR = TC_CCR_SWTRG | TC_CCR_CLKEN; // CLKEN and SWTRG
|
||||
}
|
||||
}
|
||||
// Handy function to strobe a pin, and will do so conditionally based on an int
|
||||
// and a desired int. I was using PWM channels to decide what to strobe, thus
|
||||
// the variable names. If you want to just strobe a pin leave the ints blank
|
||||
void strobePin(uint32_t pin, uint32_t desiredChannel = 0,
|
||||
uint32_t channel = 0) {
|
||||
if (channel == desiredChannel) {
|
||||
digitalWrite(pin, HIGH);
|
||||
digitalWrite(pin, LOW);
|
||||
}
|
||||
}
|
||||
void PWM_Handler(void) {
|
||||
// The datasheet is INCREDIBLY confusing here again. It writes of interrupts
|
||||
// on comparison registers, and tells us that each channel has comparison
|
||||
// registers, and proceeds to tell us that interrupts are only for synchronous
|
||||
// channels. This is contradictory at best, useless at worst! The truth is
|
||||
// that Atmel's tech writers are atrocious. The PWM unit containts two almost
|
||||
// distinct peripherals. Theres the PWM generator with its 8 channels and
|
||||
// individual register and clock selection. In addition to this, attached to
|
||||
// channel 0 only is the "comparison" peripheral. Even though the main PWM
|
||||
// channel registers will compare values against the clock, do not call them
|
||||
// comparison registers. They are Period and Duty cycle registers. Thats all
|
||||
// they can be used for, so don't think of them as "comparison", especially
|
||||
// since there are "comparison" registers which are only good for comparison
|
||||
// attached to channel 0. The comparison unit is used to generate "special"
|
||||
// interrupts, all controleld by the IXR2 registers (where X can be E, D, M,
|
||||
// or S to make up the entire register set). The fact they are tied to
|
||||
// channel 0 is why there is the confusion mention of "synchronous" in the
|
||||
// datasheet, despite synchronous being used to mean tying main PWM output
|
||||
// channels to the same clock so that either their left sides are aligned or
|
||||
// their centers are aligned (depending on a register setting).
|
||||
//
|
||||
// The IXR1 (again E,D,M,and S) registers are for the main PWM channels
|
||||
// only, and do not make use of the comparison unit. They have 2 possible
|
||||
// sources, channel fault, and "clock event" was used because there are two
|
||||
// possible modes for the clock to be run in, but in the description, this
|
||||
// isn't explained. It really ought to be called "Period Reset". Effectively
|
||||
// this is what it is whether in left aligned or center aligned modes. In
|
||||
// the timing diagrams on page 980 of the data sheet, its clear that it
|
||||
// responds directly to the period register, not the clock itself. The clock
|
||||
// also responds to the period register, so naming of this interrupt/event
|
||||
// would make *FAR* more sense to be tied to the register than effects it the
|
||||
// most! So without checking the datasheet, its an interrupt when the period
|
||||
// expires. In center aligned the counter counts up until it matches the
|
||||
// period register, then back to 0 to make "one full period". When it hits 0
|
||||
// the "Counter Event" interrupt for the channel "CHIDx" is fired, again,
|
||||
// think of it as Period Reset interrupt. In left Aligned the counter counts
|
||||
// up until it reaches the period register and then resets to 0, and fires
|
||||
// the "CHIDx" interrupt" (period reset interrupt). That was way more
|
||||
// description than should have been necessary, but I wanted to clear up the
|
||||
// nearly worthless datasheet. It is supposed to be a reference, you're not
|
||||
// supposed to need to read the entire thing, correleate all discrepencies,
|
||||
// determine the truth, then apply, it should be a reference meaning each
|
||||
// section should be able to be read/understood individually. Intel does a
|
||||
// fantastic job of this with IA32 Software Developer's Manual! Atmel could
|
||||
// take a lesson! Even AMD with Radeon GPU documenation was better than this!
|
||||
|
||||
// Anyhow, this ISR without the complexity of IXR2 registers is actually quite
|
||||
// simple. Figure out which channel had a period reset event, and service it.
|
||||
// Since we have to read the entire ISR1 register, we need to make sure we
|
||||
// don't have multiple period update events. It should be rare, but it can
|
||||
// happen! Then, because we only use this for pausing, and each pause has one
|
||||
// period cycle, we simply advance rp, and decide what to do from there (pause
|
||||
// again or switch to outputting steps). Pretty easy :)
|
||||
|
||||
uint32_t sr = PWM_INTERFACE->PWM_ISR1;
|
||||
// uint32_t sr2 = PWM_INTERFACE->PWM_ISR2;
|
||||
uint8_t leading_zeros;
|
||||
sr = sr & 0xFF; // Ignore the faults, they will screw this up, and we
|
||||
// shouldn't be getting faults...
|
||||
while ((leading_zeros = __CLZ(sr)) < 32) {
|
||||
uint8_t channel = 32 - leading_zeros - 1;
|
||||
uint32_t mask = (1 << channel);
|
||||
sr = sr & (~mask);
|
||||
uint8_t queue_num = TimerChannel_Map[channel];
|
||||
if (channelsUsed[channel] == false) continue;
|
||||
StepperQueue* q = &fas_queue[queue_num];
|
||||
// if (q->_skipNextPWMInterrupt)
|
||||
// The idea of just blindly telling the microcontroller to skip an
|
||||
// interrupt is not possible. It should be totally deterministic to the
|
||||
// code, but it isn't. Something else is going on under the hood masked
|
||||
// from "userland" code. For some reason, one channel or the other
|
||||
// generates a spurious interrupt, but rarely do both. This makes no
|
||||
// sense. They are supposed to be independent. So, rather than just
|
||||
// saying, this is my first time re-enabling the interrupt, and its going
|
||||
// to fire, so skip it, I instead need to do some math on execution time.
|
||||
// If we're really close to when the interrupt was enabled, we should skip.
|
||||
// I've "guessed" at 100 microseconds. This is irrelevant to high rate
|
||||
// pulses, so we're only concered with pauses, which are going to be
|
||||
// measured in milliseconds, so 100 microseconds ought to be really close!
|
||||
// but 1/10th of a millisecond shouldn't warrant a delay. Seems like a
|
||||
// good balance. Hopefully this strategy proves correct for all cases...
|
||||
uint32_t t = micros();
|
||||
uint32_t timeElapsed = micros() - q->timePWMInterruptEnabled;
|
||||
if (t < q->timePWMInterruptEnabled) // Timer wrapped...
|
||||
{
|
||||
timeElapsed = 0xFFFFFFFF - q->timePWMInterruptEnabled;
|
||||
timeElapsed += t;
|
||||
}
|
||||
if (timeElapsed < 100) {
|
||||
// This is unfortunately still entirely necessary. I had thought I
|
||||
// could gate this in other ways. Nope, the ISR is low lag enough we
|
||||
// easily get into this ISR immediatly after enabling it for the pulse
|
||||
// that was just handled by the PIO ISR, so we still need to skip one.
|
||||
// Being too fast is a good thing :)
|
||||
continue;
|
||||
}
|
||||
if (!q->_pauseCommanded) {
|
||||
// In the immortal words of Richard Gray aka levelord,
|
||||
//"YOU'RE NOT SUPPOSED TO BE HERE!"
|
||||
|
||||
// try disabling the interrupt again...I saw on the logic analyzer
|
||||
// we got in here a good 5 times extra! Seems like its not always
|
||||
// disabling when we tell it to. Based on the quality of the datasheet
|
||||
// I'm not altogether surprised! Handle it and move on...
|
||||
PWM_INTERFACE->PWM_IDR1 = mask;
|
||||
continue;
|
||||
}
|
||||
PWMCHANNELMAPPING* mapping = &gChannelMap[queue_num];
|
||||
q->driver_data = (void*)mapping;
|
||||
|
||||
Pio* port = mapping->port;
|
||||
|
||||
// Now with the queue, we can get the current entry, and see if we need to
|
||||
uint8_t rp = q->read_idx;
|
||||
if (rp == q->next_write_idx) {
|
||||
// I believe this is solved by the gating of _delayCommanded and
|
||||
// hasISRActive, but its not a bad idea to double check!
|
||||
// Double interrupt case, bail before we destroy the read index...
|
||||
// We're going to write rp and wp out as pulses onto pin32.
|
||||
continue;
|
||||
}
|
||||
rp = ++q->read_idx;
|
||||
struct queue_entry* e = &q->entry[rp & QUEUE_LEN_MASK];
|
||||
if (rp == q->next_write_idx) {
|
||||
q->_hasISRactive = false;
|
||||
// Since we arent sure about more pauses, we need to say no we aren't
|
||||
// pausing anymore...
|
||||
q->_pauseCommanded = false;
|
||||
// We're disconnected already, so we don't need to worry about that
|
||||
// disconnect the interrupt though.
|
||||
PWM_INTERFACE->PWM_IDR1 = mask;
|
||||
PWM_INTERFACE->PWM_DIS = mask;
|
||||
disconnectPWMPeriphal(port, mapping->pin, mapping->channelMask);
|
||||
PWM_INTERFACE->PWM_IDR1 = mask;
|
||||
q->read_idx = rp;
|
||||
pinMode(mapping->pin, OUTPUT);
|
||||
PIO_SetOutput(mapping->port, g_APinDescription[mapping->pin].ulPin, 0, 0,
|
||||
0);
|
||||
|
||||
continue; // We're done apparently
|
||||
}
|
||||
if (e->steps > 0) {
|
||||
// stop the interrupt on the PWM generator, set the period,
|
||||
// re-attach the PIO handler, disconnect the PWM generator
|
||||
// interrupt, and send it on its way...One problem, we have to use
|
||||
// this ordering, but that means we miss an interrupt. So
|
||||
// decrement the ticks here.
|
||||
|
||||
// This delay is absolutely necessary. Our interrupt latency is
|
||||
// quite low compared to the period of the PWM output. So we're able
|
||||
// to get in here, determine our pause is done, switch the pin back to
|
||||
// the PWM output, and output the pulse before it goes low, LONG before
|
||||
// it goes low.
|
||||
// So we add a 10 microsecond delay knowing the delay will be longer
|
||||
// than 10 microseconds to ensure we have passed the point where the
|
||||
// pulse is high! We won't be far off, so we aren't killing the processor
|
||||
// with ISR time. If there were 2+ cores, I'd have another thread that
|
||||
// would do the waiting so the ISR could be as short as possible, but in
|
||||
// reality,10 microseonds would probably be shorter than the time it
|
||||
// would take to setup a timer channel, enable its interrupt, and get into
|
||||
// the ISR, so we'd likely be *less* efficient than just delaying.
|
||||
delayMicroseconds(10);
|
||||
PWM_INTERFACE->PWM_CH_NUM[channel].PWM_CPRDUPD =
|
||||
e->ticks; // Change the period first!
|
||||
PWM_INTERFACE->PWM_IDR1 = mask;
|
||||
uint32_t dwSR = port->PIO_ABSR;
|
||||
port->PIO_ABSR = (g_APinDescription[mapping->pin].ulPin | dwSR);
|
||||
// Remove the pins from under the control of PIO
|
||||
port->PIO_ODR |= g_APinDescription[mapping->pin].ulPin;
|
||||
port->PIO_PDR = g_APinDescription[mapping->pin].ulPin;
|
||||
port->PIO_IER = g_APinDescription[mapping->pin].ulPin;
|
||||
q->_pauseCommanded = false;
|
||||
} else {
|
||||
// We needed to pause longer, so set the period, and wait...
|
||||
// set the period, reconnect, restart, then disconnect PWM interrupt, so
|
||||
// the next interrupt goes to the PIO interrupt handler and works like
|
||||
// normal
|
||||
PWM_INTERFACE->PWM_CH_NUM[channel].PWM_CPRDUPD = e->ticks;
|
||||
}
|
||||
q->read_idx = rp;
|
||||
}
|
||||
}
|
||||
|
||||
inline void disconnectPWMPeriphal(Pio* port, uint8_t pin,
|
||||
uint32_t channelMask) {
|
||||
// This function has become quite minimal. Its still somewhat nice to have
|
||||
// it seperated out, but I may decided it needs to just be inline...2 lines
|
||||
// saved and function call overhead added :-/
|
||||
PWM_INTERFACE->PWM_DIS = channelMask;
|
||||
PIO_SetOutput(port, g_APinDescription[pin].ulPin, 0, 0, 0);
|
||||
}
|
||||
|
||||
// gin66: removed the obsolete clearISR flag
|
||||
inline void attachPWMPeripheral(Pio* port, uint8_t pin, uint32_t channelMask) {
|
||||
PWM_INTERFACE->PWM_IDR1 = (channelMask);
|
||||
uint32_t dwSR = port->PIO_ABSR;
|
||||
port->PIO_ABSR = (g_APinDescription[pin].ulPin | dwSR);
|
||||
// Remove the pins from under the control of PIO
|
||||
port->PIO_ODR |= g_APinDescription[pin].ulPin;
|
||||
port->PIO_PDR |= g_APinDescription[pin].ulPin;
|
||||
port->PIO_IER |= g_APinDescription[pin].ulPin;
|
||||
PWM_INTERFACE->PWM_ENA |= channelMask;
|
||||
}
|
||||
#define ISRQueueName(Q) Queue##Q##ISR
|
||||
#define ISRQueueNameStr(Q) "Queue" #Q "ISR"
|
||||
|
||||
#define DUE_STEPPER_ISR(Q) \
|
||||
void Queue##Q##ISR() { \
|
||||
/*These need only be looked up once rather than multiple times in the \
|
||||
* isr*/ \
|
||||
/*One would hope the compiler would do this, but how is it supposed to*/ \
|
||||
/*know if these values change or not? const requires setting it at */ \
|
||||
/*instantiation or casting...this is the next best thing :) */ \
|
||||
static StepperQueue* const q = &fas_queue[Q]; \
|
||||
const PWMCHANNELMAPPING* mapping = \
|
||||
(const PWMCHANNELMAPPING*)fas_queue[Q].driver_data; \
|
||||
static uint32_t channel = mapping->channel; \
|
||||
static uint32_t samPin = g_APinDescription[mapping->pin].ulPin; \
|
||||
static Pio* port = mapping->port; \
|
||||
if (!q->_hasISRactive || q->_pauseCommanded) { \
|
||||
/*In the immortal words of Richard Gray aka levelord, */ \
|
||||
/*"YOU'RE NOT SUPPOSED TO BE HERE!" */ \
|
||||
return; \
|
||||
} \
|
||||
IncrementQueue(Q); \
|
||||
uint8_t rp = q->read_idx; \
|
||||
/*This case should hopefully be elimiated....Unfortunately, it is not :(*/ \
|
||||
/*It is quite rare though.*/ \
|
||||
if (rp == q->next_write_idx) { \
|
||||
/*Double interrupt - bail before we destroy the read index!*/ \
|
||||
return; \
|
||||
} \
|
||||
struct queue_entry* e = &q->entry[rp & QUEUE_LEN_MASK]; \
|
||||
uint8_t s = --e->steps; \
|
||||
/*Obvious case, if we have done all of the steps in this queue entry....*/ \
|
||||
if (s == 0) { \
|
||||
/*Setup for the next move...The PWM Peripheral is pretty smart. We set \
|
||||
the period "update" register, and it will take effect at the end of \
|
||||
the current period. Since we arent changing the clock, there is no \
|
||||
need to change the duty cycle, just the period... \
|
||||
Check if we need to toggle direction....If so, we will also need to \
|
||||
delay...Does this ever happen? I think the dirPinBusy function \
|
||||
prevents this...*/ \
|
||||
if (e->toggle_dir) { \
|
||||
e->toggle_dir = 0; \
|
||||
*q->_dirPinPort ^= q->_dirPinMask; \
|
||||
} \
|
||||
/*Check immediatly if we need to turn off the pulse generator.Time is \
|
||||
critical! */ \
|
||||
rp++; \
|
||||
if (rp == q->next_write_idx) { \
|
||||
delayMicroseconds(10); \
|
||||
disconnectPWMPeriphal(mapping->port, mapping->pin, \
|
||||
mapping->channelMask); \
|
||||
q->_hasISRactive = false; \
|
||||
q->_connected = false; \
|
||||
/*Update the queue pointer...setup for the next move.*/ \
|
||||
q->read_idx = rp; \
|
||||
return; \
|
||||
} \
|
||||
/*Since we're done with e, e is now e_next...*/ \
|
||||
e = &q->entry[rp & QUEUE_LEN_MASK]; \
|
||||
q->read_idx = rp; \
|
||||
/*We need to look for queue entries with some sort of command \
|
||||
in them..*/ \
|
||||
if (e->steps == 0 || !e->hasSteps) { \
|
||||
delayMicroseconds(7); \
|
||||
q->_pauseCommanded = true; \
|
||||
q->timePWMInterruptEnabled = micros(); \
|
||||
PWM_INTERFACE->PWM_IER1 = mapping->channelMask; \
|
||||
port->PIO_CODR = samPin; \
|
||||
port->PIO_OER = samPin; \
|
||||
port->PIO_PER = samPin; \
|
||||
port->PIO_IDR = samPin; \
|
||||
PWM_INTERFACE->PWM_CH_NUM[channel].PWM_CPRDUPD = e->ticks; \
|
||||
return; \
|
||||
} \
|
||||
/*That should be it...We should start the next command on the next \
|
||||
cycle */ \
|
||||
AddToTotalSteps(Q, e->steps); \
|
||||
PWM_INTERFACE->PWM_CH_NUM[channel].PWM_CPRDUPD = e->ticks; \
|
||||
if (e->toggle_dir) { \
|
||||
e->toggle_dir = 0; \
|
||||
*q->_dirPinPort ^= q->_dirPinMask; \
|
||||
delayMicroseconds(30); \
|
||||
} \
|
||||
return; \
|
||||
} \
|
||||
}
|
||||
|
||||
DUE_STEPPER_ISR(0)
|
||||
DUE_STEPPER_ISR(1)
|
||||
DUE_STEPPER_ISR(2)
|
||||
DUE_STEPPER_ISR(3)
|
||||
DUE_STEPPER_ISR(4)
|
||||
DUE_STEPPER_ISR(5)
|
||||
|
||||
inline uint32_t pinToChannel(uint32_t pin) {
|
||||
switch (pin) {
|
||||
case 34:
|
||||
case 67:
|
||||
case 73:
|
||||
case 35:
|
||||
return 0;
|
||||
case 17:
|
||||
case 36:
|
||||
case 72:
|
||||
case 37:
|
||||
case 42:
|
||||
return 1;
|
||||
case 38:
|
||||
case 43:
|
||||
case 63:
|
||||
case 39:
|
||||
return 2;
|
||||
case 40:
|
||||
case 64:
|
||||
case 69:
|
||||
case 41:
|
||||
return 3;
|
||||
case 9:
|
||||
return 4;
|
||||
case 8:
|
||||
case 44:
|
||||
return 5;
|
||||
case 7:
|
||||
case 45:
|
||||
return 6;
|
||||
case 6:
|
||||
return 7;
|
||||
}
|
||||
return 0xFFFFFFFF;
|
||||
}
|
||||
|
||||
void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) {
|
||||
_queue_num = queue_num;
|
||||
driver_data = (void*)&gChannelMap[queue_num];
|
||||
_initVars();
|
||||
_step_pin = step_pin;
|
||||
channelsUsed[pinToChannel(step_pin)] = true;
|
||||
numChannels++;
|
||||
#if defined(KEEP_SCORE)
|
||||
totalPulsesDetected[queue_num] = 0;
|
||||
totalSteps[queue_num] = 0;
|
||||
#endif
|
||||
gChannelMap[queue_num].pin = step_pin;
|
||||
gChannelMap[queue_num].channel = pinToChannel(step_pin);
|
||||
TimerChannel_Map[gChannelMap[queue_num].channel] = queue_num;
|
||||
gChannelMap[queue_num].port = g_APinDescription[step_pin].pPort;
|
||||
gChannelMap[queue_num].channelMask = 1 << gChannelMap[queue_num].channel;
|
||||
static bool isPWMEnabled = false;
|
||||
if (!isPWMEnabled) {
|
||||
pmc_enable_periph_clk(PWM_INTERFACE_ID);
|
||||
// ESP32 operates at 3x the speed as the due, which is reasonable to say the
|
||||
// esp32 should be 3x as capable. In the code for the esp, the comment
|
||||
// says the clock is 160/5, but the prescaler is set to 5-1? so its at
|
||||
// 40MHz. We'll pick a slower clock rate for for this 255 for the prescaler,
|
||||
// and a mod clock of 4 for 84,000,000/4=21,000,000/255 is 82352.9....52
|
||||
// might be the better number...This puts us at roughly 1/2 the performance
|
||||
// of the esp32...Will have to see how many channels it can tolerate...
|
||||
// remain compatible with due - We'll use MCK or clk B as necessary.
|
||||
// PWMC_ConfigureClocks(PWM_FREQUENCY * PWM_MAX_DUTY_CYCLE, VARIANT_MCK / 4,
|
||||
// VARIANT_MCK);
|
||||
isPWMEnabled = true;
|
||||
}
|
||||
static bool isTCEnabled = false;
|
||||
if (!isTCEnabled) {
|
||||
// Lets set it up on a 2ms timer. That should keep the queue full...
|
||||
// Enable the peripheral
|
||||
// gin66: Better to remove any Serial.println() cause an application may
|
||||
// choose to not use Serial
|
||||
// Yes, agreed. I had meant to do that a long time ago!
|
||||
// Serial.println("Enabling Timer Channel 5");
|
||||
pmc_enable_periph_clk(ID_TC5);
|
||||
TC1->TC_CHANNEL[2].TC_CCR = 1;
|
||||
TC1->TC_CHANNEL[2].TC_CMR = 0;
|
||||
TC1->TC_CHANNEL[2].TC_CMR = 0x80 | 0x40; // Up counter reset on C
|
||||
// comparison
|
||||
// We want 2ms, we're using MCK/2 - 42MHz, so we need a count of 84000
|
||||
// Set A B and C registers so we guarantee a hit :)
|
||||
TC1->TC_CHANNEL[2].TC_RA = 84000;
|
||||
TC1->TC_CHANNEL[2].TC_RB = 84000;
|
||||
TC1->TC_CHANNEL[2].TC_RC = 84000;
|
||||
TC1->TC_CHANNEL[2].TC_IER = 0;
|
||||
TC1->TC_CHANNEL[2].TC_IER = TC_IER_CPCS;
|
||||
TC1->TC_CHANNEL[2].TC_CCR = TC_CCR_SWTRG | TC_CCR_CLKEN;
|
||||
NVIC_SetPriority(TC5_IRQn, 1);
|
||||
NVIC_EnableIRQ(TC5_IRQn);
|
||||
}
|
||||
|
||||
digitalWrite(step_pin, LOW);
|
||||
pinMode(step_pin, OUTPUT);
|
||||
NVIC_SetPriority(PIOA_IRQn, 0);
|
||||
NVIC_SetPriority(PIOB_IRQn, 0);
|
||||
NVIC_SetPriority(PIOC_IRQn, 0);
|
||||
NVIC_SetPriority(PIOD_IRQn, 0);
|
||||
PWM_INTERFACE->PWM_IDR2 = 0x00FFFF0F;
|
||||
PWM_INTERFACE->PWM_IDR1 = 0x00FFFF0F;
|
||||
NVIC_EnableIRQ(PWM_IRQn);
|
||||
PWM_INTERFACE->PWM_IDR2 = 0x00FFFF0F;
|
||||
PWM_INTERFACE->PWM_IDR1 = 0x00FFFF0F;
|
||||
|
||||
connect();
|
||||
}
|
||||
|
||||
void StepperQueue::connect() {
|
||||
PIO_Configure(g_APinDescription[_step_pin].pPort,
|
||||
g_APinDescription[_step_pin].ulPinType,
|
||||
g_APinDescription[_step_pin].ulPin,
|
||||
g_APinDescription[_step_pin].ulPinConfiguration);
|
||||
const PWMCHANNELMAPPING* mapping = (const PWMCHANNELMAPPING*)driver_data;
|
||||
PWMC_ConfigureChannel(PWM_INTERFACE, mapping->channel, PWM_CMR_CPRE_MCK_DIV_4,
|
||||
0, 0);
|
||||
// 21 pulses makes for a microsecond pulse. I believe my drivers need 5us.
|
||||
// I'll set this to 10 just to make sure the drivers receive this. At 20us
|
||||
// pulse speed, this is still a 1/2 duty cycle PWM. Should be easy for any
|
||||
// driver to pickup.
|
||||
|
||||
PWM_INTERFACE->PWM_IDR2 = 0x00FFFF0F;
|
||||
PWM_INTERFACE->PWM_IDR1 = 0x00FFFF0F;
|
||||
PWM_INTERFACE->PWM_DIS = PWM_INTERFACE->PWM_DIS & (~mapping->channelMask);
|
||||
PWMC_SetPeriod(PWM_INTERFACE, mapping->channel, 65535);
|
||||
PWM_INTERFACE->PWM_DIS = PWM_INTERFACE->PWM_DIS & (~mapping->channelMask);
|
||||
PWM_INTERFACE->PWM_CH_NUM[mapping->channel].PWM_CDTY = 21 * 10;
|
||||
PWM_INTERFACE->PWM_DIS = PWM_INTERFACE->PWM_DIS & (~mapping->channelMask);
|
||||
PWM_INTERFACE->PWM_IDR2 = 0x00FFFF0F;
|
||||
PWM_INTERFACE->PWM_IDR1 = 0x00FFFF0F;
|
||||
|
||||
// Rising edge, so we get the interrupt at the beginning of the pulse (gives
|
||||
// us some extra time) and so we only get 1 int/pulse Change would give 2.
|
||||
switch (_queue_num) {
|
||||
case 0:
|
||||
attachInterrupt(digitalPinToInterrupt(_step_pin), ISRQueueName(0),
|
||||
RISING);
|
||||
break;
|
||||
case 1:
|
||||
attachInterrupt(digitalPinToInterrupt(_step_pin), ISRQueueName(1),
|
||||
RISING);
|
||||
break;
|
||||
case 2:
|
||||
attachInterrupt(digitalPinToInterrupt(_step_pin), ISRQueueName(2),
|
||||
RISING);
|
||||
break;
|
||||
case 3:
|
||||
attachInterrupt(digitalPinToInterrupt(_step_pin), ISRQueueName(3),
|
||||
RISING);
|
||||
break;
|
||||
case 4:
|
||||
attachInterrupt(digitalPinToInterrupt(_step_pin), ISRQueueName(4),
|
||||
RISING);
|
||||
break;
|
||||
case 5:
|
||||
attachInterrupt(digitalPinToInterrupt(_step_pin), ISRQueueName(5),
|
||||
RISING);
|
||||
break;
|
||||
}
|
||||
_connected = true;
|
||||
PWM_INTERFACE->PWM_IDR2 = 0x00FFFF0F;
|
||||
PWM_INTERFACE->PWM_IDR1 = 0x00FFFF0F;
|
||||
}
|
||||
|
||||
void StepperQueue::disconnect() {
|
||||
const PWMCHANNELMAPPING* mapping = (const PWMCHANNELMAPPING*)driver_data;
|
||||
PWMC_DisableChannel(PWM_INTERFACE, mapping->channel);
|
||||
PWM_INTERFACE->PWM_DIS = PWM_INTERFACE->PWM_DIS & (~mapping->channelMask);
|
||||
|
||||
// gin66: Shouldn't there be a detachInterrupt() in order to not have stray
|
||||
// interrupt ?
|
||||
// disconnect is a strange term for what we're doing. Instead of
|
||||
// disconnecting interrupts, we disable the source of the interrupts. If
|
||||
// something external causes a signal on the pin, yes, we will get an
|
||||
// interrupt. That should never happen...but there is code in the interrupt
|
||||
// to detect it because I used that very issue to debug the delay code :) If
|
||||
// I got a pulse while in delay mode, I knew there was a problem. I had it
|
||||
// strobe a pin I could see on the logic analyzer. It made it very easy to
|
||||
// see when "wrong" behavior was happening.
|
||||
_connected = false;
|
||||
}
|
||||
|
||||
// gin66: I have reworked all code to only use startQueue().
|
||||
// This appears to be less complicated.
|
||||
// I reworked the rework...sorry, it was easier to start with something that
|
||||
// worked and then remove unnecessary code as there was clearly unnecessary
|
||||
// code :)
|
||||
void StepperQueue::startQueue() {
|
||||
// This is called only, if isRunning() == false
|
||||
noInterrupts();
|
||||
struct queue_entry* e = &entry[read_idx & QUEUE_LEN_MASK];
|
||||
// Somewhere there must be a call to nointerrupts, because addinf this solved
|
||||
// the no-start issue.
|
||||
interrupts();
|
||||
_hasISRactive = true;
|
||||
|
||||
const PWMCHANNELMAPPING* mapping = (const PWMCHANNELMAPPING*)driver_data;
|
||||
|
||||
// I had this reversed, but the situation where we are starting, but the
|
||||
// PWM peripheral is running already doesn't come up often. If the
|
||||
// channel is enabled, we need to use the UPD register to trigger the
|
||||
// update on the next cycle, otherwise we can directly set PWM_CPRD.
|
||||
// Setting the UPD register instead of the direct register fortunately wasn't
|
||||
// harmful, and works as expected. I'd still leave this check in here just
|
||||
// in case, though I believe everything is controlled well enough to not need
|
||||
// it.
|
||||
|
||||
if (PWM_INTERFACE->PWM_SR & (1 << mapping->channel)) {
|
||||
PWM_INTERFACE->PWM_CH_NUM[mapping->channel].PWM_CPRDUPD = e->ticks;
|
||||
} else {
|
||||
PWM_INTERFACE->PWM_CH_NUM[mapping->channel].PWM_CPRD = e->ticks;
|
||||
}
|
||||
|
||||
if (e->steps > 0 || e->hasSteps) {
|
||||
attachPWMPeripheral(mapping->port, mapping->pin, mapping->channelMask);
|
||||
return;
|
||||
} else {
|
||||
// I could see the confusion...man was I tired of this code when I
|
||||
// committed :) It was quite dirty. This case is when we DO NOT
|
||||
// want to fully attach the PWM peripheral, in that we do not want it
|
||||
// outputting anything. So we set the period, but do not attach it. You
|
||||
// moved the period to the top, and I changed it to be the period change
|
||||
// with the check to see if we were already running, which is unlikely
|
||||
// here, but better to be certain. The rest of this function is about
|
||||
// making certain the output is disabled, but the PWM interrupt used for
|
||||
// delays only is enabled and working.
|
||||
//
|
||||
// I had found that every single move command always started with a delay.
|
||||
// So even in startQueue, this is necessary. I had seen that even before
|
||||
// the change for adding delays for the dir pin! So this block is still
|
||||
// entirely necessary :) I'll make better comments though in the code
|
||||
// below...
|
||||
|
||||
// Disconnect the step pin entirely, and force its output to 0! We're
|
||||
// in a delay, we do not want extra pulses generated!
|
||||
PIO_SetOutput(mapping->port, g_APinDescription[mapping->pin].ulPin, 0, 0,
|
||||
0);
|
||||
// Disable the PIO interrupt. We probably do not need to do this, and I
|
||||
// tested my test code without it, but it is still probably the "right
|
||||
// thing" to do. Don't jump into an interrupt unless we specifically
|
||||
// say its ok to do so! :)
|
||||
mapping->port->PIO_IDR = g_APinDescription[mapping->pin].ulPin;
|
||||
// Here we enable the PWM peripheral, despite having its output
|
||||
// disconnected. This is how we manage the delays. The pwm peripheral
|
||||
// happily counts the time until its supposed to generate a pulse. It
|
||||
// generates the pulse into a high impedance output, because the PIO
|
||||
// module has it switched off, but it still generates a PWM interrupt
|
||||
// for us, so we know to advance the queue using the PWM ISR.
|
||||
PWM_INTERFACE->PWM_ENA |= mapping->channelMask;
|
||||
_pauseCommanded = true;
|
||||
// We do not want to set timePWMInterruptEnabled here. It takes a bit to
|
||||
// explain, but the purpose of the timePWMInterruptEnabled is to check to
|
||||
// see if we are in the PWM interrupt for the same interrupt as the pulse.
|
||||
// That seems strange and it is. I could probably have used the PWM
|
||||
// interrupts only for this purpose, but it was actually nice to seperate
|
||||
// the delay ISR from the pulse ISR. It made things clean...up until it
|
||||
// didn't. We would enable the PWM ISR and it would immediatly trigger
|
||||
// from the PIO ISR. The only reliable way to deal with it was to see if
|
||||
// we were within a hundred microseconds of the pulse. If we were, we
|
||||
// skipped that ISR call. Without it, we'd end up skipping the delay.
|
||||
// Enabling the check here, well, we don't seem to get to the ISR within
|
||||
// 100 microseconds anyhow, so it...shouldn't....cause any issues if
|
||||
// present, but why tempt fate/Murphy? We never want to skip a PWM
|
||||
// interrupt at this point, so lets not give it the opportunity :)
|
||||
|
||||
/*Enable the ISR too so we don't miss it*/
|
||||
PWM_INTERFACE->PWM_IER1 = PWM_INTERFACE->PWM_IMR1 | mapping->channelMask;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void StepperQueue::forceStop() {
|
||||
noInterrupts();
|
||||
read_idx = next_write_idx;
|
||||
interrupts();
|
||||
const PWMCHANNELMAPPING* mapping = (const PWMCHANNELMAPPING*)driver_data;
|
||||
PWMC_DisableChannel(PWM_INTERFACE, mapping->channel);
|
||||
// gin66: I have added this line in the hope to make it work
|
||||
_hasISRactive = false;
|
||||
}
|
||||
|
||||
bool StepperQueue::isValidStepPin(uint8_t step_pin) {
|
||||
if (pinToChannel(step_pin) < 0x08) {
|
||||
if (!channelsUsed[pinToChannel(step_pin)]) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool StepperQueue::isRunning() { return _hasISRactive; }
|
||||
|
||||
int8_t StepperQueue::queueNumForStepPin(uint8_t step_pin) { return -1; }
|
||||
|
||||
void StepperQueue::adjustSpeedToStepperCount(uint8_t steppers) {
|
||||
max_speed_in_ticks = 420; // This equals 50kHz @ 21MHz
|
||||
}
|
||||
|
||||
void fas_init_engine(FastAccelStepperEngine* engine, uint8_t cpu_core) {
|
||||
fas_engine = engine;
|
||||
}
|
||||
#endif
|
||||
142
src/StepperISR_esp32.cpp
Normal file
142
src/StepperISR_esp32.cpp
Normal file
@@ -0,0 +1,142 @@
|
||||
#include "StepperISR.h"
|
||||
|
||||
#if defined(SUPPORT_ESP32)
|
||||
|
||||
// Here are the global variables to interface with the interrupts
|
||||
StepperQueue fas_queue[NUM_QUEUES];
|
||||
|
||||
void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) {
|
||||
uint8_t channel = queue_num;
|
||||
#ifdef SUPPORT_ESP32_MCPWM_PCNT
|
||||
if (channel < QUEUES_MCPWM_PCNT) {
|
||||
use_rmt = false;
|
||||
init_mcpwm_pcnt(channel, step_pin);
|
||||
return;
|
||||
}
|
||||
channel -= QUEUES_MCPWM_PCNT;
|
||||
#endif
|
||||
#ifdef SUPPORT_ESP32_RMT
|
||||
use_rmt = true;
|
||||
init_rmt(channel, step_pin);
|
||||
#endif
|
||||
}
|
||||
|
||||
void StepperQueue::connect() {
|
||||
#ifdef SUPPORT_ESP32_RMT
|
||||
if (use_rmt) {
|
||||
connect_rmt();
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
#ifdef SUPPORT_ESP32_MCPWM_PCNT
|
||||
connect_mcpwm_pcnt();
|
||||
#endif
|
||||
}
|
||||
|
||||
void StepperQueue::disconnect() {
|
||||
#ifdef SUPPORT_ESP32_RMT
|
||||
if (use_rmt) {
|
||||
disconnect_rmt();
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
#ifdef SUPPORT_ESP32_MCPWM_PCNT
|
||||
disconnect_mcpwm_pcnt();
|
||||
#endif
|
||||
}
|
||||
|
||||
bool StepperQueue::isReadyForCommands() const {
|
||||
#if defined(SUPPORT_ESP32_RMT) && defined(SUPPORT_ESP32_MCPWM_PCNT)
|
||||
if (use_rmt) {
|
||||
return isReadyForCommands_rmt();
|
||||
}
|
||||
return isReadyForCommands_mcpwm_pcnt();
|
||||
#elif defined(SUPPORT_ESP32_MCPWM_PCNT)
|
||||
return isReadyForCommands_mcpwm_pcnt();
|
||||
#elif defined(SUPPORT_ESP32_RMT)
|
||||
return isReadyForCommands_rmt();
|
||||
#else
|
||||
#error "Nothing defined here"
|
||||
#endif
|
||||
}
|
||||
|
||||
void StepperQueue::startQueue() {
|
||||
#ifdef SUPPORT_ESP32_RMT
|
||||
if (use_rmt) {
|
||||
startQueue_rmt();
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
#ifdef SUPPORT_ESP32_MCPWM_PCNT
|
||||
startQueue_mcpwm_pcnt();
|
||||
#endif
|
||||
}
|
||||
void StepperQueue::forceStop() {
|
||||
#ifdef SUPPORT_ESP32_RMT
|
||||
if (use_rmt) {
|
||||
forceStop_rmt();
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
#ifdef SUPPORT_ESP32_MCPWM_PCNT
|
||||
forceStop_mcpwm_pcnt();
|
||||
#endif
|
||||
}
|
||||
uint16_t StepperQueue::_getPerformedPulses() const {
|
||||
#ifdef SUPPORT_ESP32_RMT
|
||||
if (use_rmt) {
|
||||
return _getPerformedPulses_rmt();
|
||||
}
|
||||
#endif
|
||||
#ifdef SUPPORT_ESP32_MCPWM_PCNT
|
||||
return _getPerformedPulses_mcpwm_pcnt();
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
|
||||
//*************************************************************************************************
|
||||
|
||||
bool StepperQueue::isValidStepPin(uint8_t step_pin) {
|
||||
gpio_drive_cap_t strength;
|
||||
esp_err_t res = gpio_get_drive_capability((gpio_num_t)step_pin, &strength);
|
||||
return res == ESP_OK;
|
||||
}
|
||||
int8_t StepperQueue::queueNumForStepPin(uint8_t step_pin) { return -1; }
|
||||
|
||||
//*************************************************************************************************
|
||||
[[noreturn]] void StepperTask(void *parameter) {
|
||||
auto *engine = (FastAccelStepperEngine *)parameter;
|
||||
const TickType_t delay_4ms =
|
||||
(DELAY_MS_BASE + portTICK_PERIOD_MS - 1) / portTICK_PERIOD_MS;
|
||||
while (true) {
|
||||
engine->manageSteppers();
|
||||
#if ESP_IDF_VERSION_MAJOR == 4
|
||||
// not clear, if the wdt reset is needed. With idf-version 5, the reset
|
||||
// causes an issue.
|
||||
esp_task_wdt_reset();
|
||||
#endif
|
||||
vTaskDelay(delay_4ms);
|
||||
}
|
||||
}
|
||||
|
||||
void StepperQueue::adjustSpeedToStepperCount(uint8_t steppers) {
|
||||
max_speed_in_ticks = 80; // This equals 200kHz @ 16MHz
|
||||
}
|
||||
|
||||
void fas_init_engine(FastAccelStepperEngine *engine, uint8_t cpu_core) {
|
||||
#if ESP_IDF_VERSION_MAJOR == 4
|
||||
#define STACK_SIZE 2000
|
||||
#define PRIORITY configMAX_PRIORITIES
|
||||
#else
|
||||
#define STACK_SIZE 3000
|
||||
#define PRIORITY (configMAX_PRIORITIES - 1)
|
||||
#endif
|
||||
if (cpu_core > 1) {
|
||||
xTaskCreate(StepperTask, "StepperTask", STACK_SIZE, engine, PRIORITY, nullptr);
|
||||
} else {
|
||||
xTaskCreatePinnedToCore(StepperTask, "StepperTask", STACK_SIZE, engine,
|
||||
PRIORITY, nullptr, cpu_core);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
574
src/StepperISR_idf4_esp32_mcpwm_pcnt.cpp
Normal file
574
src/StepperISR_idf4_esp32_mcpwm_pcnt.cpp
Normal file
@@ -0,0 +1,574 @@
|
||||
#include "StepperISR.h"
|
||||
#if defined(SUPPORT_ESP32_MCPWM_PCNT) && (ESP_IDF_VERSION_MAJOR == 4)
|
||||
|
||||
#define DEFAULT_TIMER_H_L_TRANSITION 160
|
||||
|
||||
// cannot be updated while timer is running => fix it to 0
|
||||
#define TIMER_PRESCALER 0
|
||||
|
||||
// #define TEST_PROBE 18
|
||||
|
||||
// Here the associated mapping from queue to mcpwm/pcnt units
|
||||
//
|
||||
// As the ISR is accessing this table, the mapping cannot be put into flash,
|
||||
// even this is actually a constant table
|
||||
struct mapping_s {
|
||||
mcpwm_unit_t mcpwm_unit;
|
||||
uint8_t timer;
|
||||
mcpwm_io_signals_t pwm_output_pin;
|
||||
pcnt_unit_t pcnt_unit;
|
||||
uint8_t input_sig_index;
|
||||
uint32_t cmpr_tea_int_clr;
|
||||
uint32_t cmpr_tea_int_ena;
|
||||
uint32_t cmpr_tea_int_raw;
|
||||
};
|
||||
|
||||
static struct mapping_s channel2mapping[NUM_QUEUES] = {
|
||||
{
|
||||
mcpwm_unit : MCPWM_UNIT_0,
|
||||
timer : 0,
|
||||
pwm_output_pin : MCPWM0A,
|
||||
pcnt_unit : PCNT_UNIT_0,
|
||||
input_sig_index : PCNT_SIG_CH0_IN0_IDX,
|
||||
cmpr_tea_int_clr : MCPWM_OP0_TEA_INT_CLR,
|
||||
cmpr_tea_int_ena : MCPWM_OP0_TEA_INT_ENA,
|
||||
cmpr_tea_int_raw : MCPWM_OP0_TEA_INT_RAW
|
||||
},
|
||||
{
|
||||
mcpwm_unit : MCPWM_UNIT_0,
|
||||
timer : 1,
|
||||
pwm_output_pin : MCPWM1A,
|
||||
pcnt_unit : PCNT_UNIT_1,
|
||||
input_sig_index : PCNT_SIG_CH0_IN1_IDX,
|
||||
cmpr_tea_int_clr : MCPWM_OP1_TEA_INT_CLR,
|
||||
cmpr_tea_int_ena : MCPWM_OP1_TEA_INT_ENA,
|
||||
cmpr_tea_int_raw : MCPWM_OP1_TEA_INT_RAW
|
||||
},
|
||||
{
|
||||
mcpwm_unit : MCPWM_UNIT_0,
|
||||
timer : 2,
|
||||
pwm_output_pin : MCPWM2A,
|
||||
pcnt_unit : PCNT_UNIT_2,
|
||||
input_sig_index : PCNT_SIG_CH0_IN2_IDX,
|
||||
cmpr_tea_int_clr : MCPWM_OP2_TEA_INT_CLR,
|
||||
cmpr_tea_int_ena : MCPWM_OP2_TEA_INT_ENA,
|
||||
cmpr_tea_int_raw : MCPWM_OP2_TEA_INT_RAW
|
||||
},
|
||||
{
|
||||
mcpwm_unit : MCPWM_UNIT_1,
|
||||
timer : 0,
|
||||
pwm_output_pin : MCPWM0A,
|
||||
pcnt_unit : PCNT_UNIT_3,
|
||||
input_sig_index : PCNT_SIG_CH0_IN3_IDX,
|
||||
cmpr_tea_int_clr : MCPWM_OP0_TEA_INT_CLR,
|
||||
cmpr_tea_int_ena : MCPWM_OP0_TEA_INT_ENA,
|
||||
cmpr_tea_int_raw : MCPWM_OP0_TEA_INT_RAW
|
||||
},
|
||||
#ifndef HAVE_ESP32S3_PULSE_COUNTER
|
||||
{
|
||||
mcpwm_unit : MCPWM_UNIT_1,
|
||||
timer : 1,
|
||||
pwm_output_pin : MCPWM1A,
|
||||
pcnt_unit : PCNT_UNIT_4,
|
||||
input_sig_index : PCNT_SIG_CH0_IN4_IDX,
|
||||
cmpr_tea_int_clr : MCPWM_OP1_TEA_INT_CLR,
|
||||
cmpr_tea_int_ena : MCPWM_OP1_TEA_INT_ENA,
|
||||
cmpr_tea_int_raw : MCPWM_OP1_TEA_INT_RAW
|
||||
},
|
||||
{
|
||||
mcpwm_unit : MCPWM_UNIT_1,
|
||||
timer : 2,
|
||||
pwm_output_pin : MCPWM2A,
|
||||
pcnt_unit : PCNT_UNIT_5,
|
||||
input_sig_index : PCNT_SIG_CH0_IN5_IDX,
|
||||
cmpr_tea_int_clr : MCPWM_OP2_TEA_INT_CLR,
|
||||
cmpr_tea_int_ena : MCPWM_OP2_TEA_INT_ENA,
|
||||
cmpr_tea_int_raw : MCPWM_OP2_TEA_INT_RAW
|
||||
},
|
||||
#endif
|
||||
};
|
||||
|
||||
static void IRAM_ATTR prepare_for_next_command(
|
||||
StepperQueue *queue, const struct queue_entry *e_next) {
|
||||
uint8_t next_steps = e_next->steps;
|
||||
if (next_steps > 0) {
|
||||
const struct mapping_s *mapping =
|
||||
(const struct mapping_s *)queue->driver_data;
|
||||
pcnt_unit_t pcnt_unit = mapping->pcnt_unit;
|
||||
// is updated only on zero
|
||||
#ifndef HAVE_ESP32S3_PULSE_COUNTER
|
||||
PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim = next_steps;
|
||||
#else
|
||||
PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim_un = next_steps;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
#define isr_pcnt_counter_clear(pcnt_unit) \
|
||||
REG_SET_BIT(PCNT_CTRL_REG, (1 << (2 * pcnt_unit))); \
|
||||
REG_CLR_BIT(PCNT_CTRL_REG, (1 << (2 * pcnt_unit)))
|
||||
|
||||
static void IRAM_ATTR apply_command(StepperQueue *queue,
|
||||
const struct queue_entry *e) {
|
||||
const struct mapping_s *mapping =
|
||||
(const struct mapping_s *)queue->driver_data;
|
||||
mcpwm_unit_t mcpwm_unit = mapping->mcpwm_unit;
|
||||
mcpwm_dev_t *mcpwm = mcpwm_unit == MCPWM_UNIT_0 ? &MCPWM0 : &MCPWM1;
|
||||
pcnt_unit_t pcnt_unit = mapping->pcnt_unit;
|
||||
uint8_t timer = mapping->timer;
|
||||
uint8_t steps = e->steps;
|
||||
if (e->toggle_dir) {
|
||||
LL_TOGGLE_PIN(queue->dirPin);
|
||||
}
|
||||
uint16_t ticks = e->ticks;
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
if (mcpwm->timer[timer].status.value <= 1) { // mcpwm Timer is stopped ?
|
||||
mcpwm->timer[timer].period.upmethod = 0; // 0 = immediate update, 1 = TEZ
|
||||
#else /* __ESP32_IDF_V44__ */
|
||||
if (mcpwm->timer[timer].timer_status.timer_value <=
|
||||
1) { // mcpwm Timer is stopped ?
|
||||
mcpwm->timer[timer].timer_cfg0.timer_period_upmethod =
|
||||
0; // 0 = immediate update, 1 = TEZ
|
||||
#endif /* __ESP32_IDF_V44__ */
|
||||
} else {
|
||||
// 0 = immediate update, 1 = TEZ
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
mcpwm->timer[timer].period.upmethod = 1;
|
||||
#else /* __ESP32_IDF_V44__ */
|
||||
mcpwm->timer[timer].timer_cfg0.timer_period_upmethod = 1;
|
||||
#endif /* __ESP32_IDF_V44__ */
|
||||
}
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
mcpwm->timer[timer].period.period = ticks;
|
||||
#else /* __ESP32_IDF_V44__ */
|
||||
mcpwm->timer[timer].timer_cfg0.timer_period = ticks;
|
||||
#endif /* __ESP32_IDF_V44__ */
|
||||
if (steps == 0) {
|
||||
// timer value = 1 - upcounting: output low
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
mcpwm->channel[timer].generator[0].utea = 1;
|
||||
#else /* __ESP32_IDF_V44__ */
|
||||
mcpwm->operators[timer].generator[0].gen_utea = 1;
|
||||
#endif /* __ESP32_IDF_V44__ */
|
||||
mcpwm->int_clr.val = mapping->cmpr_tea_int_clr;
|
||||
mcpwm->int_ena.val |= mapping->cmpr_tea_int_ena;
|
||||
} else {
|
||||
bool disable_mcpwm_interrupt = true;
|
||||
#ifndef HAVE_ESP32S3_PULSE_COUNTER
|
||||
if (PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim != steps) {
|
||||
#else
|
||||
if (PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim_un != steps) {
|
||||
#endif
|
||||
// For fast pulses, eventually the ISR is late. So take the current pulse
|
||||
// count into consideration.
|
||||
//
|
||||
// Automatic update for next pulse cnt on pulse counter zero does not
|
||||
// work. For example the sequence:
|
||||
// 5 pulses
|
||||
// 1 pause ==> here need to store 3, but not
|
||||
// available yet 3 pulses
|
||||
//
|
||||
// Read counter
|
||||
#ifndef HAVE_ESP32S3_PULSE_COUNTER
|
||||
uint16_t val1 = steps - PCNT.cnt_unit[pcnt_unit].cnt_val;
|
||||
#else
|
||||
uint16_t val1 = steps - PCNT.cnt_unit[pcnt_unit].pulse_cnt_un;
|
||||
#endif
|
||||
// Clear flag for l-->h transition
|
||||
mcpwm->int_clr.val = mapping->cmpr_tea_int_clr;
|
||||
// Read counter again
|
||||
#ifndef HAVE_ESP32S3_PULSE_COUNTER
|
||||
uint16_t val2 = steps - PCNT.cnt_unit[pcnt_unit].cnt_val;
|
||||
#else
|
||||
uint16_t val2 = steps - PCNT.cnt_unit[pcnt_unit].pulse_cnt_un;
|
||||
#endif
|
||||
// If no pulse arrives between val1 and val2:
|
||||
// val2 == val1
|
||||
// If pulse arrives between val1 and int_clr:
|
||||
// mcpwm status is cleared and val2 == val1+1
|
||||
// If pulse arrives between int_clr and val2:
|
||||
// mcpwm status is set and val2 == val1+1
|
||||
// => mcwpm status info is not reliable, so clear again
|
||||
if (val1 != val2) {
|
||||
// Clear flag again. No pulse can be expected between val2 and here
|
||||
mcpwm->int_clr.val = mapping->cmpr_tea_int_clr;
|
||||
}
|
||||
|
||||
// is updated only on zero
|
||||
#ifndef HAVE_ESP32S3_PULSE_COUNTER
|
||||
PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim = val2;
|
||||
#else
|
||||
PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim_un = val2;
|
||||
#endif
|
||||
// force take over
|
||||
isr_pcnt_counter_clear(pcnt_unit);
|
||||
// Check, if pulse has come in
|
||||
if ((mcpwm->int_raw.val & mapping->cmpr_tea_int_raw) != 0) {
|
||||
// Pulse has come in
|
||||
// Check if the pulse has been counted or not
|
||||
#ifndef HAVE_ESP32S3_PULSE_COUNTER
|
||||
if (PCNT.cnt_unit[pcnt_unit].cnt_val == 0) {
|
||||
#else
|
||||
if (PCNT.cnt_unit[pcnt_unit].pulse_cnt_un == 0) {
|
||||
#endif
|
||||
// pulse hasn't been counted, so adjust the limit
|
||||
// is updated only on zero
|
||||
#ifndef HAVE_ESP32S3_PULSE_COUNTER
|
||||
PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim = val2 - 1;
|
||||
#else
|
||||
PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim_un = val2 - 1;
|
||||
#endif
|
||||
// force take over
|
||||
isr_pcnt_counter_clear(pcnt_unit);
|
||||
|
||||
// In case val2 == 1, then there may be a problem.
|
||||
// The pulse counter may not cause any interrupt with cnt_h_lim == 0
|
||||
// and if the mcpwm-interrupt is disabled, then the queue is stalled.
|
||||
// in this case, we should leave the mcpwm enabled
|
||||
disable_mcpwm_interrupt = val2 > 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
// disable mcpwm interrupt
|
||||
if (disable_mcpwm_interrupt) {
|
||||
mcpwm->int_ena.val &= ~mapping->cmpr_tea_int_ena;
|
||||
}
|
||||
// timer value = 1 - upcounting: output high
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
mcpwm->channel[timer].generator[0].utea = 2;
|
||||
#else /* __ESP32_IDF_V44__ */
|
||||
mcpwm->operators[timer].generator[0].gen_utea = 2;
|
||||
#endif /* __ESP32_IDF_V44__ */
|
||||
}
|
||||
}
|
||||
|
||||
static void IRAM_ATTR init_stop(StepperQueue *q) {
|
||||
// init stop is normally called after the first command,
|
||||
// because the second command is entered too late
|
||||
// and after the last command aka running out of commands.
|
||||
const struct mapping_s *mapping = (const struct mapping_s *)q->driver_data;
|
||||
mcpwm_unit_t mcpwm_unit = mapping->mcpwm_unit;
|
||||
mcpwm_dev_t *mcpwm = mcpwm_unit == MCPWM_UNIT_0 ? &MCPWM0 : &MCPWM1;
|
||||
uint8_t timer = mapping->timer;
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
mcpwm->timer[timer].mode.start = 0; // 0: stop at TEZ
|
||||
#else /* __ESP32_IDF_V44__ */
|
||||
mcpwm->timer[timer].timer_cfg1.timer_start = 0; // 0: stop at TEZ
|
||||
#endif /* __ESP32_IDF_V44__ */
|
||||
// timer value = 1 - upcounting: output low
|
||||
mcpwm->int_ena.val &= ~mapping->cmpr_tea_int_ena;
|
||||
// PCNT.conf_unit[mapping->pcnt_unit].conf2.cnt_h_lim = 1;
|
||||
q->_isRunning = false;
|
||||
}
|
||||
|
||||
static void IRAM_ATTR what_is_next(StepperQueue *q) {
|
||||
// when starting the queue, apply_command for the first entry is called.
|
||||
// the read pointer stays at this position. This function is reached,
|
||||
// if the command to which read pointer points to is completed.
|
||||
bool isPrepared = q->_nextCommandIsPrepared;
|
||||
q->_nextCommandIsPrepared = false;
|
||||
uint8_t rp = q->read_idx;
|
||||
if (rp != q->next_write_idx) {
|
||||
struct queue_entry *e_completed = &q->entry[rp & QUEUE_LEN_MASK];
|
||||
bool repeat_entry = e_completed->repeat_entry != 0;
|
||||
if (!repeat_entry) {
|
||||
rp++;
|
||||
q->read_idx = rp;
|
||||
}
|
||||
if (rp != q->next_write_idx) {
|
||||
struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK];
|
||||
if (!isPrepared) {
|
||||
prepare_for_next_command(q, e_curr); // a no-op for pause command
|
||||
const struct mapping_s *mapping =
|
||||
(const struct mapping_s *)q->driver_data;
|
||||
isr_pcnt_counter_clear(mapping->pcnt_unit);
|
||||
}
|
||||
apply_command(q, e_curr);
|
||||
if (!repeat_entry) {
|
||||
rp++;
|
||||
if (rp != q->next_write_idx) {
|
||||
struct queue_entry *e_next = &q->entry[rp & QUEUE_LEN_MASK];
|
||||
q->_nextCommandIsPrepared = true;
|
||||
prepare_for_next_command(q, e_next); // a no-op for pause command
|
||||
}
|
||||
} else {
|
||||
q->_nextCommandIsPrepared = false;
|
||||
}
|
||||
return;
|
||||
}
|
||||
}
|
||||
// no more commands: stop timer at period end
|
||||
init_stop(q);
|
||||
}
|
||||
|
||||
static void IRAM_ATTR pcnt_isr_service(void *arg) {
|
||||
StepperQueue *q = (StepperQueue *)arg;
|
||||
what_is_next(q);
|
||||
}
|
||||
|
||||
// MCPWM_SERVICE is only used in case of pause
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
|
||||
#define MCPWM_SERVICE(mcpwm, TIMER, pcnt) \
|
||||
if (mcpwm.int_st.cmpr##TIMER##_tea_int_st != 0) { \
|
||||
/*managed in apply_command() */ \
|
||||
/*mcpwm.int_clr.cmpr##TIMER##_tea_int_clr = 1;*/ \
|
||||
StepperQueue *q = &fas_queue[pcnt]; \
|
||||
what_is_next(q); \
|
||||
}
|
||||
|
||||
#else /* __ESP32_IDF_V44__ */
|
||||
|
||||
#define MCPWM_SERVICE(mcpwm, TIMER, pcnt) \
|
||||
if (mcpwm.int_st.op##TIMER##_tea_int_st != 0) { \
|
||||
/*managed in apply_command() */ \
|
||||
/*mcpwm.int_clr.cmpr##TIMER##_tea_int_clr = 1;*/ \
|
||||
StepperQueue *q = &fas_queue[pcnt]; \
|
||||
what_is_next(q); \
|
||||
}
|
||||
|
||||
#endif /* __ESP32_IDF_V44__ */
|
||||
|
||||
static void IRAM_ATTR mcpwm0_isr_service(void *arg) {
|
||||
// For whatever reason, this interrupt is constantly called even with int_st =
|
||||
// 0 while the timer is running
|
||||
MCPWM_SERVICE(MCPWM0, 0, 0);
|
||||
MCPWM_SERVICE(MCPWM0, 1, 1);
|
||||
MCPWM_SERVICE(MCPWM0, 2, 2);
|
||||
}
|
||||
static void IRAM_ATTR mcpwm1_isr_service(void *arg) {
|
||||
MCPWM_SERVICE(MCPWM1, 0, 3);
|
||||
#ifndef HAVE_ESP32S3_PULSE_COUNTER
|
||||
MCPWM_SERVICE(MCPWM1, 1, 4);
|
||||
MCPWM_SERVICE(MCPWM1, 2, 5);
|
||||
#endif
|
||||
}
|
||||
|
||||
void StepperQueue::init_mcpwm_pcnt(uint8_t channel_num, uint8_t step_pin) {
|
||||
#ifdef TEST_PROBE
|
||||
pinMode(TEST_PROBE, OUTPUT);
|
||||
#endif
|
||||
|
||||
_initVars();
|
||||
_step_pin = step_pin;
|
||||
|
||||
const struct mapping_s *mapping = &channel2mapping[channel_num];
|
||||
driver_data = (void *)mapping;
|
||||
|
||||
mcpwm_unit_t mcpwm_unit = mapping->mcpwm_unit;
|
||||
mcpwm_dev_t *mcpwm = mcpwm_unit == MCPWM_UNIT_0 ? &MCPWM0 : &MCPWM1;
|
||||
pcnt_unit_t pcnt_unit = mapping->pcnt_unit;
|
||||
uint8_t timer = mapping->timer;
|
||||
|
||||
pcnt_config_t cfg;
|
||||
// if step_pin is not set here (or 0x30), then it does not work
|
||||
cfg.pulse_gpio_num = step_pin; // static 0 is 0x30, static 1 is 0x38
|
||||
cfg.ctrl_gpio_num = PCNT_PIN_NOT_USED; // static 0 is 0x30, static 1 is 0x38
|
||||
cfg.lctrl_mode = PCNT_MODE_KEEP;
|
||||
cfg.hctrl_mode = PCNT_MODE_KEEP;
|
||||
cfg.pos_mode = PCNT_COUNT_INC; // increment on rising edge
|
||||
cfg.neg_mode = PCNT_COUNT_DIS; // ignore falling edge
|
||||
cfg.counter_h_lim = 1;
|
||||
cfg.counter_l_lim = 0;
|
||||
cfg.unit = pcnt_unit;
|
||||
cfg.channel = PCNT_CHANNEL_0;
|
||||
pcnt_unit_config(&cfg);
|
||||
|
||||
#ifndef HAVE_ESP32S3_PULSE_COUNTER
|
||||
PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim = 1;
|
||||
PCNT.conf_unit[pcnt_unit].conf0.thr_h_lim_en = 1;
|
||||
PCNT.conf_unit[pcnt_unit].conf0.thr_l_lim_en = 0;
|
||||
#else
|
||||
PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim_un = 1;
|
||||
PCNT.conf_unit[pcnt_unit].conf0.thr_h_lim_en_un = 1;
|
||||
PCNT.conf_unit[pcnt_unit].conf0.thr_l_lim_en_un = 0;
|
||||
#endif
|
||||
|
||||
pcnt_counter_clear(pcnt_unit);
|
||||
pcnt_counter_resume(pcnt_unit);
|
||||
pcnt_event_enable(pcnt_unit, PCNT_EVT_H_LIM);
|
||||
if (channel_num == 0) {
|
||||
// isr_service_install apparently enables the interrupt
|
||||
PCNT.int_clr.val = PCNT.int_st.val;
|
||||
pcnt_isr_service_install(ESP_INTR_FLAG_SHARED | ESP_INTR_FLAG_IRAM);
|
||||
}
|
||||
pcnt_isr_handler_add(pcnt_unit, pcnt_isr_service, (void *)this);
|
||||
|
||||
if (timer == 0) {
|
||||
// Init mcwpm module for use
|
||||
periph_module_enable(mcpwm_unit == MCPWM_UNIT_0 ? PERIPH_PWM0_MODULE
|
||||
: PERIPH_PWM1_MODULE);
|
||||
mcpwm->int_ena.val = 0; // disable all interrupts
|
||||
mcpwm_isr_register(
|
||||
mcpwm_unit,
|
||||
mcpwm_unit == MCPWM_UNIT_0 ? mcpwm0_isr_service : mcpwm1_isr_service,
|
||||
NULL, ESP_INTR_FLAG_IRAM | ESP_INTR_FLAG_SHARED, NULL);
|
||||
|
||||
// 160 MHz/5 = 32 MHz => 16 MHz in up/down-mode
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
mcpwm->clk_cfg.prescale = 5 - 1;
|
||||
#else /* __ESP32_IDF_V44__ */
|
||||
mcpwm->clk_cfg.clk_prescale = 5 - 1;
|
||||
#endif /* __ESP32_IDF_V44__ */
|
||||
|
||||
// timer 0 is input for operator 0
|
||||
// timer 1 is input for operator 1
|
||||
// timer 2 is input for operator 2
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
mcpwm->timer_sel.operator0_sel = 0;
|
||||
mcpwm->timer_sel.operator1_sel = 1;
|
||||
mcpwm->timer_sel.operator2_sel = 2;
|
||||
#else /* __ESP32_IDF_V44__ */
|
||||
mcpwm->operator_timersel.operator0_timersel = 0;
|
||||
mcpwm->operator_timersel.operator1_timersel = 1;
|
||||
mcpwm->operator_timersel.operator2_timersel = 2;
|
||||
#endif /* __ESP32_IDF_V44__ */
|
||||
}
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
mcpwm->timer[timer].period.upmethod = 1; // 0 = immediate update, 1 = TEZ
|
||||
mcpwm->timer[timer].period.prescale = TIMER_PRESCALER;
|
||||
mcpwm->timer[timer].period.period = 400; // Random value
|
||||
mcpwm->timer[timer].mode.mode = 3; // 3=up/down counting
|
||||
mcpwm->timer[timer].mode.start = 0; // 0: stop at TEZ
|
||||
#else /* __ESP32_IDF_V44__ */
|
||||
// 0 = immediate update, 1 = TEZ
|
||||
mcpwm->timer[timer].timer_cfg0.timer_period_upmethod = 1;
|
||||
mcpwm->timer[timer].timer_cfg0.timer_prescale = TIMER_PRESCALER;
|
||||
mcpwm->timer[timer].timer_cfg0.timer_period = 400; // Random value
|
||||
mcpwm->timer[timer].timer_cfg1.timer_mod = 3; // 3=up/down counting
|
||||
mcpwm->timer[timer].timer_cfg1.timer_start = 0; // 0: stop at TEZ
|
||||
#endif /* __ESP32_IDF_V44__ */
|
||||
|
||||
// this sequence should reset the timer to 0
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
mcpwm->timer[timer].sync.timer_phase = 0; // prepare value of 0
|
||||
mcpwm->timer[timer].sync.in_en = 1; // enable sync
|
||||
mcpwm->timer[timer].sync.sync_sw ^= 1; // force a sync
|
||||
mcpwm->timer[timer].sync.in_en = 0; // disable sync
|
||||
|
||||
mcpwm->channel[timer].cmpr_cfg.a_upmethod = 0; // 0 = immediate update
|
||||
mcpwm->channel[timer].cmpr_value[0].cmpr_val = 1; // set compare value A
|
||||
mcpwm->channel[timer].generator[0].val = 0; // clear all trigger actions
|
||||
mcpwm->channel[timer].generator[1].val = 0; // clear all trigger actions
|
||||
mcpwm->channel[timer].generator[0].dtep = 1; // low at period
|
||||
mcpwm->channel[timer].db_cfg.val = 0; // edge delay disabled
|
||||
mcpwm->channel[timer].carrier_cfg.val = 0; // carrier disabled
|
||||
#else /* __ESP32_IDF_V44__ */
|
||||
mcpwm->timer[timer].timer_sync.timer_phase = 0; // prepare value of 0
|
||||
mcpwm->timer[timer].timer_sync.timer_synci_en = 1; // enable sync
|
||||
mcpwm->timer[timer].timer_sync.timer_sync_sw ^= 1; // force a sync
|
||||
mcpwm->timer[timer].timer_sync.timer_synci_en = 0; // disable sync
|
||||
|
||||
mcpwm->operators[timer].gen_stmp_cfg.gen_a_upmethod =
|
||||
0; // 0 = immediate update
|
||||
mcpwm->operators[timer].timestamp[0].gen = 1; // set compare value A
|
||||
mcpwm->operators[timer].generator[0].val = 0; // clear all trigger actions
|
||||
mcpwm->operators[timer].generator[1].val = 0; // clear all trigger actions
|
||||
mcpwm->operators[timer].generator[0].gen_dtep = 1; // low at period
|
||||
mcpwm->operators[timer].dt_cfg.val = 0; // edge delay disabled
|
||||
mcpwm->operators[timer].carrier_cfg.val = 0; // carrier disabled
|
||||
#endif /* __ESP32_IDF_V44__ */
|
||||
|
||||
digitalWrite(step_pin, LOW);
|
||||
pinMode(step_pin, OUTPUT);
|
||||
|
||||
// at last, link mcpwm to output pin and back into pcnt input
|
||||
connect();
|
||||
}
|
||||
|
||||
void StepperQueue::connect_mcpwm_pcnt() {
|
||||
const struct mapping_s *mapping = (const struct mapping_s *)driver_data;
|
||||
mcpwm_unit_t mcpwm_unit = mapping->mcpwm_unit;
|
||||
mcpwm_gpio_init(mcpwm_unit, mapping->pwm_output_pin, _step_pin);
|
||||
// Doesn't work with gpio_matrix_in
|
||||
// gpio_matrix_in(step_pin, mapping->input_sig_index, false);
|
||||
gpio_iomux_in(_step_pin, mapping->input_sig_index);
|
||||
}
|
||||
|
||||
void StepperQueue::disconnect_mcpwm_pcnt() {
|
||||
// sig_index = 0x100 => cancel output
|
||||
gpio_matrix_out(_step_pin, 0x100, false, false);
|
||||
// untested alternative:
|
||||
// gpio_reset_pin((gpio_num_t)q->step_pin);
|
||||
}
|
||||
|
||||
// Mechanism is like this, starting from stopped motor:
|
||||
//
|
||||
// * init counter
|
||||
// * init mcpwm
|
||||
// * start mcpwm
|
||||
// * -- pcnt counter counts every L->H-transition at mcpwm.timer = 1
|
||||
// * -- if counter reaches planned steps, then counter is reset and
|
||||
// * interrupt is created
|
||||
//
|
||||
// * pcnt interrupt: available time is from mcpwm.timer = 1+x to period
|
||||
// - read next commmand: store period in counter shadow and
|
||||
// steps in pcnt
|
||||
// - without next command: set mcpwm to stop mode on reaching
|
||||
// period
|
||||
//
|
||||
|
||||
void StepperQueue::startQueue_mcpwm_pcnt() {
|
||||
#ifdef TEST_PROBE
|
||||
// The time used by this command can have an impact
|
||||
digitalWrite(TEST_PROBE, digitalRead(TEST_PROBE) == HIGH ? LOW : HIGH);
|
||||
#endif
|
||||
const struct mapping_s *mapping = (const struct mapping_s *)driver_data;
|
||||
mcpwm_unit_t mcpwm_unit = mapping->mcpwm_unit;
|
||||
mcpwm_dev_t *mcpwm = mcpwm_unit == MCPWM_UNIT_0 ? &MCPWM0 : &MCPWM1;
|
||||
uint8_t timer = mapping->timer;
|
||||
|
||||
// apply_command() assumes the pcnt counter to contain executed steps
|
||||
// and deduct this from the new command. For a starting motor
|
||||
// need to make sure, that the counter is 0. (issue #33)
|
||||
pcnt_unit_t pcnt_unit = mapping->pcnt_unit;
|
||||
pcnt_counter_clear(pcnt_unit);
|
||||
|
||||
_isRunning = true;
|
||||
_nextCommandIsPrepared = false;
|
||||
struct queue_entry *e = &entry[read_idx & QUEUE_LEN_MASK];
|
||||
apply_command(this, e);
|
||||
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
mcpwm->timer[timer].mode.start = 2; // 2=run continuous
|
||||
#else /* __ESP32_IDF_V44__ */
|
||||
mcpwm->timer[timer].timer_cfg1.timer_start = 2; // 2=run continuous
|
||||
#endif /* __ESP32_IDF_V44__ */
|
||||
}
|
||||
void StepperQueue::forceStop_mcpwm_pcnt() {
|
||||
init_stop(this);
|
||||
read_idx = next_write_idx;
|
||||
}
|
||||
bool StepperQueue::isReadyForCommands_mcpwm_pcnt() {
|
||||
if (isRunning()) {
|
||||
return true;
|
||||
}
|
||||
const struct mapping_s *mapping = (const struct mapping_s *)driver_data;
|
||||
mcpwm_unit_t mcpwm_unit = mapping->mcpwm_unit;
|
||||
mcpwm_dev_t *mcpwm = mcpwm_unit == MCPWM_UNIT_0 ? &MCPWM0 : &MCPWM1;
|
||||
uint8_t timer = mapping->timer;
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
if (mcpwm->timer[timer].status.value > 1) {
|
||||
#else /* __ESP32_IDF_V44__ */
|
||||
if (mcpwm->timer[timer].timer_status.timer_value > 1) {
|
||||
#endif /* __ESP32_IDF_V44__ */
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
// #ifndef __ESP32_IDF_V44__
|
||||
// return (mcpwm->timer[timer].mode.start != 2); // 2=run continuous
|
||||
// #else /* __ESP32_IDF_V44__ */
|
||||
// return (mcpwm->timer[timer].timer_cfg1.timer_start != 2); // 2=run
|
||||
// continuous
|
||||
// #endif /* __ESP32_IDF_V44__ */
|
||||
}
|
||||
uint16_t StepperQueue::_getPerformedPulses_mcpwm_pcnt() {
|
||||
const struct mapping_s *mapping = (const struct mapping_s *)driver_data;
|
||||
#ifndef HAVE_ESP32S3_PULSE_COUNTER
|
||||
return PCNT.cnt_unit[mapping->pcnt_unit].cnt_val;
|
||||
#else
|
||||
return PCNT.cnt_unit[mapping->pcnt_unit].pulse_cnt_un;
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif
|
||||
511
src/StepperISR_idf4_esp32_rmt.cpp
Normal file
511
src/StepperISR_idf4_esp32_rmt.cpp
Normal file
@@ -0,0 +1,511 @@
|
||||
#include "StepperISR.h"
|
||||
#if defined(HAVE_ESP32_RMT) && (ESP_IDF_VERSION_MAJOR == 4)
|
||||
|
||||
// #define TEST_MODE
|
||||
|
||||
#include "fas_arch/test_probe.h"
|
||||
|
||||
// The following concept is in use:
|
||||
//
|
||||
// The buffer of 64Bytes is split into two parts à 31 words.
|
||||
// Each part will hold one command (or part of).
|
||||
// After the 2*31 words an end marker is placed.
|
||||
// The threshold is set to 31.
|
||||
//
|
||||
// This way, the threshold interrupt occurs after the first part
|
||||
// and the end interrupt after the second part.
|
||||
//
|
||||
// Of these 32 bits, the low 16-bit entry is sent first and the high entry
|
||||
// second.
|
||||
// Every 16 bit entry defines with MSB the output level and the lower 15 bits
|
||||
// the ticks.
|
||||
#define PART_SIZE 31
|
||||
|
||||
void IRAM_ATTR StepperQueue::stop_rmt(bool both) {
|
||||
// We are stopping the rmt by letting it run into the end at high speed.
|
||||
//
|
||||
// disable the interrupts
|
||||
// rmt_set_tx_intr_en(channel, false);
|
||||
// rmt_set_tx_thr_intr_en(channel, false, 0);
|
||||
|
||||
// stop esp32 rmt, by let it hit the end
|
||||
RMT.conf_ch[channel].conf1.tx_conti_mode = 0;
|
||||
|
||||
// replace second part of buffer with pauses
|
||||
uint32_t *data = FAS_RMT_MEM(channel);
|
||||
uint8_t start = both ? 0 : PART_SIZE;
|
||||
data = &data[start];
|
||||
for (uint8_t i = start; i < 2 * PART_SIZE; i++) {
|
||||
// two pauses à n ticks to achieve MIN_CMD_TICKS
|
||||
*data++ = 0x00010001 * ((MIN_CMD_TICKS + 61) / 62);
|
||||
}
|
||||
*data = 0;
|
||||
|
||||
// as the rmt is not running anymore, mark it as stopped
|
||||
_rmtStopped = true;
|
||||
}
|
||||
|
||||
static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one,
|
||||
uint32_t *data) {
|
||||
if (!fill_part_one) {
|
||||
data += PART_SIZE;
|
||||
}
|
||||
uint8_t rp = q->read_idx;
|
||||
if (rp == q->next_write_idx) {
|
||||
// no command in queue
|
||||
if (fill_part_one) {
|
||||
q->bufferContainsSteps[0] = false;
|
||||
for (uint8_t i = 0; i < PART_SIZE; i++) {
|
||||
// make a pause with approx. 1ms
|
||||
// 258 ticks * 2 * 31 = 15996 @ 16MHz
|
||||
*data++ = 0x01020102;
|
||||
}
|
||||
} else {
|
||||
q->stop_rmt(false);
|
||||
}
|
||||
return;
|
||||
}
|
||||
// Process command
|
||||
struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK];
|
||||
|
||||
if (e_curr->toggle_dir) {
|
||||
// the command requests dir pin toggle
|
||||
// This is ok only, if the ongoing command does not contain steps
|
||||
if (q->bufferContainsSteps[fill_part_one ? 1 : 0]) {
|
||||
// So we need a pause. change the finished read entry into a pause
|
||||
q->bufferContainsSteps[fill_part_one ? 0 : 1] = false;
|
||||
for (uint8_t i = 0; i < PART_SIZE; i++) {
|
||||
// two pauses à n ticks to achieve MIN_CMD_TICKS
|
||||
*data++ = 0x00010001 * ((MIN_CMD_TICKS + 61) / 62);
|
||||
}
|
||||
return;
|
||||
}
|
||||
// The ongoing command does not contain steps, so change dir here should be
|
||||
// ok. But we need the gpio_ll functions, which are always
|
||||
// inlined...hopefully.
|
||||
LL_TOGGLE_PIN(q->dirPin);
|
||||
// and delete the request
|
||||
e_curr->toggle_dir = 0;
|
||||
}
|
||||
|
||||
uint8_t steps = e_curr->steps;
|
||||
uint16_t ticks = e_curr->ticks;
|
||||
// if (steps != 0) {
|
||||
// PROBE_2_TOGGLE;
|
||||
//}
|
||||
uint32_t last_entry;
|
||||
if (steps == 0) {
|
||||
q->bufferContainsSteps[fill_part_one ? 0 : 1] = false;
|
||||
for (uint8_t i = 0; i < PART_SIZE - 1; i++) {
|
||||
// two pauses à 3 ticks. the 2 for debugging
|
||||
*data++ = 0x00010002;
|
||||
ticks -= 3;
|
||||
}
|
||||
uint16_t ticks_l = ticks >> 1;
|
||||
uint16_t ticks_r = ticks - ticks_l;
|
||||
last_entry = ticks_l;
|
||||
last_entry <<= 16;
|
||||
last_entry |= ticks_r;
|
||||
} else {
|
||||
q->bufferContainsSteps[fill_part_one ? 0 : 1] = true;
|
||||
if (ticks == 0xffff) {
|
||||
// special treatment for this case, because an rmt entry can only cover up
|
||||
// to 0xfffe ticks every step must be minimum split into two rmt entries,
|
||||
// so at max PART/2 steps can be done.
|
||||
if (steps < PART_SIZE / 2) {
|
||||
for (uint8_t i = 1; i < steps; i++) {
|
||||
// steps-1 iterations
|
||||
*data++ = 0x40007fff | 0x8000;
|
||||
*data++ = 0x20002000;
|
||||
}
|
||||
*data++ = 0x40007fff | 0x8000;
|
||||
uint16_t delta = PART_SIZE - 2 * steps;
|
||||
delta <<= 5;
|
||||
*data++ = 0x20002000 - delta;
|
||||
// 2*(steps - 1) + 1 already stored => 2*steps - 1
|
||||
// and after this for loop one entry added => 2*steps
|
||||
for (uint8_t i = 2 * steps; i < PART_SIZE - 1; i++) {
|
||||
*data++ = 0x00100010;
|
||||
}
|
||||
last_entry = 0x00100010;
|
||||
steps = 0;
|
||||
} else {
|
||||
steps -= PART_SIZE / 2;
|
||||
for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) {
|
||||
*data++ = 0x40007fff | 0x8000;
|
||||
*data++ = 0x20002000;
|
||||
}
|
||||
*data++ = 0x40007fff | 0x8000;
|
||||
last_entry = 0x20002000;
|
||||
}
|
||||
} else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) {
|
||||
uint8_t steps_to_do = steps;
|
||||
if (steps > PART_SIZE) {
|
||||
steps_to_do /= 2;
|
||||
}
|
||||
|
||||
uint16_t ticks_l = ticks >> 1;
|
||||
uint16_t ticks_r = ticks - ticks_l;
|
||||
uint32_t rmt_entry = ticks_l;
|
||||
rmt_entry <<= 16;
|
||||
rmt_entry |= ticks_r | 0x8000; // with step
|
||||
for (uint8_t i = 1; i < steps_to_do; i++) {
|
||||
*data++ = rmt_entry;
|
||||
}
|
||||
uint32_t delta = PART_SIZE - steps_to_do;
|
||||
delta <<= 18; // shift in upper 16bit and multiply with 4
|
||||
*data++ = rmt_entry - delta;
|
||||
for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) {
|
||||
*data++ = 0x00020002;
|
||||
}
|
||||
last_entry = 0x00020002;
|
||||
steps -= steps_to_do;
|
||||
} else {
|
||||
// either >= 2*PART_SIZE or = PART_SIZE
|
||||
// every entry one step
|
||||
uint16_t ticks_l = ticks >> 1;
|
||||
uint16_t ticks_r = ticks - ticks_l;
|
||||
uint32_t rmt_entry = ticks_l;
|
||||
rmt_entry <<= 16;
|
||||
rmt_entry |= ticks_r | 0x8000; // with step
|
||||
for (uint8_t i = 0; i < PART_SIZE - 1; i++) {
|
||||
*data++ = rmt_entry;
|
||||
}
|
||||
last_entry = rmt_entry;
|
||||
steps -= PART_SIZE;
|
||||
}
|
||||
}
|
||||
if (!fill_part_one) {
|
||||
// Note: When enabling the continuous transmission mode by setting
|
||||
// RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the
|
||||
// channel continuously, that is, from the first byte to the last one,
|
||||
// then from the first to the last again, and so on. In this mode, there
|
||||
// will be an idle level lasting one clk_div cycle between N and N+1
|
||||
// transmissions.
|
||||
last_entry -= 1;
|
||||
}
|
||||
*data = last_entry;
|
||||
|
||||
// Data is complete
|
||||
if (steps == 0) {
|
||||
// The command has been completed
|
||||
if (e_curr->repeat_entry == 0) {
|
||||
q->read_idx = rp + 1;
|
||||
}
|
||||
} else {
|
||||
e_curr->steps = steps;
|
||||
}
|
||||
}
|
||||
|
||||
#ifndef RMT_CHANNEL_MEM
|
||||
#define RMT_LIMIT tx_lim
|
||||
#define RMT_FIFO apb_fifo_mask
|
||||
#else
|
||||
#define RMT_LIMIT limit
|
||||
#define RMT_FIFO fifo_mask
|
||||
#endif
|
||||
|
||||
#ifdef TEST_MODE
|
||||
#define PROCESS_CHANNEL(ch) \
|
||||
if (mask & RMT_CH##ch##_TX_END_INT_ST) { \
|
||||
PROBE_2_TOGGLE; \
|
||||
} \
|
||||
if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \
|
||||
PROBE_3_TOGGLE; \
|
||||
/* now repeat the interrupt at buffer size + end marker */ \
|
||||
RMT.tx_lim_ch[ch].RMT_LIMIT = PART_SIZE * 2 + 1; \
|
||||
}
|
||||
#else
|
||||
#define PROCESS_CHANNEL(ch) \
|
||||
if (mask & RMT_CH##ch##_TX_END_INT_ST) { \
|
||||
PROBE_2_TOGGLE; \
|
||||
StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \
|
||||
if (q->_rmtStopped) { \
|
||||
rmt_set_tx_intr_en(q->channel, false); \
|
||||
rmt_set_tx_thr_intr_en(q->channel, false, PART_SIZE + 1); \
|
||||
q->_isRunning = false; \
|
||||
PROBE_1_TOGGLE; \
|
||||
} else { \
|
||||
apply_command(q, false, FAS_RMT_MEM(ch)); \
|
||||
} \
|
||||
} \
|
||||
if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \
|
||||
PROBE_3_TOGGLE; \
|
||||
StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \
|
||||
if (!q->_rmtStopped) { \
|
||||
apply_command(q, true, FAS_RMT_MEM(ch)); \
|
||||
/* now repeat the interrupt at buffer size + end marker */ \
|
||||
RMT.tx_lim_ch[ch].RMT_LIMIT = PART_SIZE * 2 + 1; \
|
||||
} \
|
||||
}
|
||||
#endif
|
||||
|
||||
static void IRAM_ATTR tx_intr_handler(void *arg) {
|
||||
uint32_t mask = RMT.int_st.val;
|
||||
RMT.int_clr.val = mask;
|
||||
PROCESS_CHANNEL(0);
|
||||
PROCESS_CHANNEL(1);
|
||||
#if QUEUES_RMT >= 4
|
||||
PROCESS_CHANNEL(2);
|
||||
PROCESS_CHANNEL(3);
|
||||
#endif
|
||||
#if QUEUES_RMT == 8
|
||||
PROCESS_CHANNEL(4);
|
||||
PROCESS_CHANNEL(5);
|
||||
PROCESS_CHANNEL(6);
|
||||
PROCESS_CHANNEL(7);
|
||||
#endif
|
||||
}
|
||||
|
||||
void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {
|
||||
#ifdef TEST_PROBE_1
|
||||
if (channel_num == 0) {
|
||||
pinMode(TEST_PROBE_1, OUTPUT);
|
||||
PROBE_1(HIGH);
|
||||
delay(10);
|
||||
PROBE_1(LOW);
|
||||
delay(5);
|
||||
PROBE_1(HIGH);
|
||||
delay(5);
|
||||
PROBE_1(LOW);
|
||||
delay(5);
|
||||
}
|
||||
#endif
|
||||
#ifdef TEST_PROBE_2
|
||||
pinMode(TEST_PROBE_2, OUTPUT);
|
||||
PROBE_2(LOW);
|
||||
#endif
|
||||
#ifdef TEST_PROBE_3
|
||||
pinMode(TEST_PROBE_3, OUTPUT);
|
||||
PROBE_3(LOW);
|
||||
#endif
|
||||
|
||||
_initVars();
|
||||
_step_pin = step_pin;
|
||||
digitalWrite(step_pin, LOW);
|
||||
pinMode(step_pin, OUTPUT);
|
||||
|
||||
channel = (rmt_channel_t)channel_num;
|
||||
|
||||
if (channel_num == 0) {
|
||||
periph_module_enable(PERIPH_RMT_MODULE);
|
||||
}
|
||||
// 80 MHz/5 = 16 MHz
|
||||
rmt_set_tx_intr_en(channel, false);
|
||||
rmt_set_tx_thr_intr_en(channel, false, PART_SIZE + 1);
|
||||
rmt_set_source_clk(channel, RMT_BASECLK_APB);
|
||||
rmt_set_clk_div(channel, 5);
|
||||
rmt_set_mem_block_num(channel, 1);
|
||||
rmt_set_tx_carrier(channel, false, 0, 0, RMT_CARRIER_LEVEL_LOW);
|
||||
rmt_tx_stop(channel);
|
||||
rmt_rx_stop(channel);
|
||||
// rmt_tx_memory_reset is not defined in arduino V340 and based on test result
|
||||
// not needed rmt_tx_memory_reset(channel);
|
||||
if (channel_num == 0) {
|
||||
rmt_isr_register(tx_intr_handler, NULL,
|
||||
ESP_INTR_FLAG_SHARED | ESP_INTR_FLAG_IRAM, NULL);
|
||||
RMT.apb_conf.RMT_FIFO = 1; // disable fifo mode
|
||||
RMT.apb_conf.mem_tx_wrap_en = 0;
|
||||
}
|
||||
|
||||
_isRunning = false;
|
||||
_rmtStopped = true;
|
||||
|
||||
connect();
|
||||
|
||||
#ifdef TEST_MODE
|
||||
if (channel == 0) {
|
||||
uint32_t *mem = FAS_RMT_MEM(channel);
|
||||
// Fill the buffer with a significant pattern for debugging
|
||||
for (uint8_t i = 0; i < 32; i += 2) {
|
||||
*mem++ = 0x7fffffff;
|
||||
*mem++ = 0x7fff7fff;
|
||||
}
|
||||
for (uint8_t i = 32; i < 62; i += 2) {
|
||||
*mem++ = 0x3fffdfff;
|
||||
*mem++ = 0x3fff3fff;
|
||||
}
|
||||
// without end marker it does not loop
|
||||
*mem++ = 0;
|
||||
*mem++ = 0;
|
||||
RMT.conf_ch[channel].conf1.tx_conti_mode = 1;
|
||||
rmt_set_tx_intr_en(channel, true);
|
||||
rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1);
|
||||
// rmt_tx_start(channel, true);
|
||||
PROBE_2_TOGGLE;
|
||||
|
||||
delay(1000);
|
||||
if (false) {
|
||||
mem--;
|
||||
mem--;
|
||||
// destroy end marker => no end interrupt, no repeat
|
||||
*mem++ = 0x00010001;
|
||||
*mem = 0x00010001;
|
||||
}
|
||||
if (true) {
|
||||
// just clear conti mode => causes end interrup, no repeat
|
||||
RMT.conf_ch[channel].conf1.tx_conti_mode = 0;
|
||||
}
|
||||
delay(1000);
|
||||
// actually no need to enable/disable interrupts.
|
||||
// and this seems to avoid some pitfalls
|
||||
|
||||
// This runs the RMT buffer once
|
||||
RMT.conf_ch[channel].conf1.tx_conti_mode = 1;
|
||||
delay(1);
|
||||
RMT.conf_ch[channel].conf1.tx_conti_mode = 0;
|
||||
while (true) {
|
||||
delay(1);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void StepperQueue::connect_rmt() {
|
||||
// rmt_set_tx_intr_en(channel, true);
|
||||
// rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1);
|
||||
RMT.conf_ch[channel].conf1.idle_out_lv = 0;
|
||||
RMT.conf_ch[channel].conf1.idle_out_en = 1;
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)_step_pin);
|
||||
#else
|
||||
rmt_set_gpio(channel, RMT_MODE_TX, (gpio_num_t)_step_pin, false);
|
||||
#endif
|
||||
}
|
||||
|
||||
void StepperQueue::disconnect_rmt() {
|
||||
rmt_set_tx_intr_en(channel, false);
|
||||
rmt_set_tx_thr_intr_en(channel, false, PART_SIZE + 1);
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
// rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)-1);
|
||||
#else
|
||||
rmt_set_gpio(channel, RMT_MODE_TX, GPIO_NUM_NC, false);
|
||||
#endif
|
||||
RMT.conf_ch[channel].conf1.idle_out_en = 0;
|
||||
}
|
||||
|
||||
void StepperQueue::startQueue_rmt() {
|
||||
// #define TRACE
|
||||
#ifdef TRACE
|
||||
Serial.println("START");
|
||||
#endif
|
||||
#ifdef TEST_PROBE_2
|
||||
PROBE_2(LOW);
|
||||
#endif
|
||||
#ifdef TEST_PROBE_3
|
||||
PROBE_3(LOW);
|
||||
#endif
|
||||
#ifdef TEST_PROBE_1
|
||||
delay(1);
|
||||
PROBE_1_TOGGLE;
|
||||
delay(1);
|
||||
PROBE_1_TOGGLE;
|
||||
delay(1);
|
||||
PROBE_1_TOGGLE;
|
||||
delay(1);
|
||||
#endif
|
||||
rmt_tx_stop(channel);
|
||||
// rmt_rx_stop(channel);
|
||||
// rmt_tx_memory_reset(channel);
|
||||
// the following assignment should not be needed;
|
||||
// RMT.data_ch[channel] = 0;
|
||||
uint32_t *mem = FAS_RMT_MEM(channel);
|
||||
// Fill the buffer with a significant pattern for debugging
|
||||
// Keep it for now
|
||||
for (uint8_t i = 0; i < 2 * PART_SIZE; i += 2) {
|
||||
mem[i] = 0x0fff8fff;
|
||||
mem[i + 1] = 0x7fff8fff;
|
||||
}
|
||||
mem[2 * PART_SIZE] = 0;
|
||||
mem[2 * PART_SIZE + 1] = 0;
|
||||
_isRunning = true;
|
||||
_rmtStopped = false;
|
||||
rmt_set_tx_intr_en(channel, false);
|
||||
rmt_set_tx_thr_intr_en(channel, false, 0);
|
||||
// RMT.apb_conf.mem_tx_wrap_en = 0;
|
||||
|
||||
// #define TRACE
|
||||
#ifdef TRACE
|
||||
Serial.print("Queue:");
|
||||
Serial.print(read_idx);
|
||||
Serial.print('/');
|
||||
Serial.println(next_write_idx);
|
||||
for (uint8_t i = 0; i < 64; i++) {
|
||||
Serial.print(i);
|
||||
Serial.print(' ');
|
||||
Serial.println(mem[i], HEX);
|
||||
}
|
||||
#endif
|
||||
|
||||
// set dirpin toggle here
|
||||
uint8_t rp = read_idx;
|
||||
if (rp == next_write_idx) {
|
||||
// nothing to do ?
|
||||
// Should not happen, so bail
|
||||
return;
|
||||
}
|
||||
if (entry[rp & QUEUE_LEN_MASK].toggle_dir) {
|
||||
LL_TOGGLE_PIN(dirPin);
|
||||
entry[rp & QUEUE_LEN_MASK].toggle_dir = false;
|
||||
}
|
||||
|
||||
bufferContainsSteps[0] = true;
|
||||
bufferContainsSteps[1] = true;
|
||||
apply_command(this, true, mem);
|
||||
|
||||
#ifdef TRACE
|
||||
Serial.print(RMT.conf_ch[channel].conf0.val, BIN);
|
||||
Serial.print(' ');
|
||||
Serial.print(RMT.conf_ch[channel].conf1.val, BIN);
|
||||
Serial.println(' ');
|
||||
Serial.print(RMT.apb_conf.mem_tx_wrap_en);
|
||||
Serial.println(' ');
|
||||
for (uint8_t i = 0; i < 64; i++) {
|
||||
Serial.print(i);
|
||||
Serial.print(' ');
|
||||
Serial.println(mem[i], HEX);
|
||||
}
|
||||
if (!isRunning()) {
|
||||
Serial.println("STOPPED 1");
|
||||
}
|
||||
#endif
|
||||
|
||||
apply_command(this, false, mem);
|
||||
|
||||
#ifdef TRACE
|
||||
Serial.print(RMT.conf_ch[channel].conf0.val, BIN);
|
||||
Serial.print(' ');
|
||||
Serial.print(RMT.conf_ch[channel].conf1.val, BIN);
|
||||
Serial.println(' ');
|
||||
Serial.print(RMT.apb_conf.mem_tx_wrap_en);
|
||||
Serial.println(' ');
|
||||
|
||||
for (uint8_t i = 0; i < 64; i++) {
|
||||
Serial.print(i);
|
||||
Serial.print(' ');
|
||||
Serial.println(mem[i], HEX);
|
||||
}
|
||||
#endif
|
||||
rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1);
|
||||
rmt_set_tx_intr_en(channel, true);
|
||||
_rmtStopped = false;
|
||||
|
||||
// This starts the rmt module
|
||||
RMT.conf_ch[channel].conf1.tx_conti_mode = 1;
|
||||
|
||||
RMT.conf_ch[channel].conf1.tx_start = 1;
|
||||
}
|
||||
void StepperQueue::forceStop_rmt() {
|
||||
stop_rmt(true);
|
||||
|
||||
// and empty the buffer
|
||||
read_idx = next_write_idx;
|
||||
}
|
||||
bool StepperQueue::isReadyForCommands_rmt() {
|
||||
if (_isRunning) {
|
||||
return !_rmtStopped;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
uint16_t StepperQueue::_getPerformedPulses_rmt() { return 0; }
|
||||
#endif
|
||||
652
src/StepperISR_idf4_esp32c3_rmt.cpp
Normal file
652
src/StepperISR_idf4_esp32c3_rmt.cpp
Normal file
@@ -0,0 +1,652 @@
|
||||
#include "StepperISR.h"
|
||||
#if defined(HAVE_ESP32C3_RMT) && (ESP_IDF_VERSION_MAJOR == 4)
|
||||
|
||||
// #define TEST_MODE
|
||||
// #define TRACE
|
||||
|
||||
#include "fas_arch/test_probe.h"
|
||||
|
||||
// The following concept is in use:
|
||||
//
|
||||
// The rmt buffer is split into two parts.
|
||||
// Each part will hold one command (or part of).
|
||||
// After the two parts an end marker is placed.
|
||||
//
|
||||
// Of these 32 bits, the low 16-bit entry is sent first and the high entry
|
||||
// second.
|
||||
// Every 16 bit entry defines with MSB the output level and the lower 15 bits
|
||||
// the ticks.
|
||||
//
|
||||
// Important difference of esp32c3 (compared to esp32):
|
||||
// - configuration updates need an conf_update strobe
|
||||
// (apparently the manual is not correct by mentioning to set conf_update
|
||||
// first)
|
||||
// - if the end marker is hit in continuous mode, there is no end interrupt
|
||||
// - there is no tick lost with the end marker
|
||||
// - minimum periods as per relation 1 and 2 to be adhered to
|
||||
//
|
||||
//
|
||||
#ifdef HAVE_ESP32C3_RMT
|
||||
#define PART_SIZE 23
|
||||
#define RMT_MEM_SIZE 48
|
||||
#else
|
||||
#error
|
||||
#define PART_SIZE 31
|
||||
#define RMT_MEM_SIZE 64
|
||||
#endif
|
||||
|
||||
// In order to avoid threshold/end interrupt on end, add one
|
||||
#define enable_rmt_interrupts(channel) \
|
||||
{ \
|
||||
RMT.tx_lim[channel].RMT_LIMIT = PART_SIZE + 2; \
|
||||
RMT.tx_conf[channel].conf_update = 1; \
|
||||
RMT.tx_conf[channel].conf_update = 0; \
|
||||
RMT.int_clr.val |= 0x101 << channel; \
|
||||
RMT.int_ena.val |= 0x101 << channel; \
|
||||
}
|
||||
#define disable_rmt_interrupts(channel) \
|
||||
{ RMT.int_ena.val &= ~(0x101 << channel); }
|
||||
|
||||
void IRAM_ATTR StepperQueue::stop_rmt(bool both) {
|
||||
// We are stopping the rmt by letting it run into the end at high speed.
|
||||
//
|
||||
// disable the interrupts
|
||||
// rmt_set_tx_intr_en(channel, false);
|
||||
// rmt_set_tx_thr_intr_en(channel, false, 0);
|
||||
|
||||
// stop esp32 rmt, by let it hit the end
|
||||
RMT.tx_conf[channel].tx_conti_mode = 0;
|
||||
RMT.tx_conf[channel].conf_update = 1;
|
||||
RMT.tx_conf[channel].conf_update = 0;
|
||||
|
||||
// replace second part of buffer with pauses
|
||||
uint32_t *data = FAS_RMT_MEM(channel);
|
||||
uint8_t start = both ? 0 : PART_SIZE;
|
||||
data = &data[start];
|
||||
for (uint8_t i = start; i < 2 * PART_SIZE; i++) {
|
||||
// two pauses à n ticks to achieve MIN_CMD_TICKS
|
||||
*data++ = 0x00010001 * ((MIN_CMD_TICKS + 61) / 62);
|
||||
}
|
||||
*data = 0;
|
||||
|
||||
// as the rmt is not running anymore, mark it as stopped
|
||||
_rmtStopped = true;
|
||||
}
|
||||
|
||||
static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one,
|
||||
uint32_t *data) {
|
||||
if (!fill_part_one) {
|
||||
data += PART_SIZE;
|
||||
}
|
||||
uint8_t rp = q->read_idx;
|
||||
if (rp == q->next_write_idx) {
|
||||
// no command in queue
|
||||
if (fill_part_one) {
|
||||
q->bufferContainsSteps[0] = false;
|
||||
for (uint8_t i = 0; i < PART_SIZE; i++) {
|
||||
// make a pause with approx. 1ms
|
||||
// 258 ticks * 2 * 31 = 15996 @ 16MHz
|
||||
// 347 ticks * 2 * 23 = 15962 @ 16MHz
|
||||
*data++ = 0x010001 * (16000 / 2 / PART_SIZE);
|
||||
}
|
||||
} else {
|
||||
q->stop_rmt(false);
|
||||
}
|
||||
return;
|
||||
}
|
||||
// Process command
|
||||
struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK];
|
||||
|
||||
if (e_curr->toggle_dir) {
|
||||
// the command requests dir pin toggle
|
||||
// This is ok only, if the ongoing command does not contain steps
|
||||
if (q->bufferContainsSteps[fill_part_one ? 1 : 0]) {
|
||||
// So we need a pause. change the finished read entry into a pause
|
||||
q->bufferContainsSteps[fill_part_one ? 0 : 1] = false;
|
||||
for (uint8_t i = 0; i < PART_SIZE; i++) {
|
||||
// two pauses à n ticks to achieve MIN_CMD_TICKS
|
||||
*data++ = 0x00010001 *
|
||||
((MIN_CMD_TICKS + 2 * PART_SIZE - 1) / (2 * PART_SIZE));
|
||||
}
|
||||
return;
|
||||
}
|
||||
// The ongoing command does not contain steps, so change dir here should be
|
||||
// ok
|
||||
LL_TOGGLE_PIN(q->dirPin);
|
||||
// and delete the request
|
||||
e_curr->toggle_dir = 0;
|
||||
}
|
||||
|
||||
uint8_t steps = e_curr->steps;
|
||||
uint16_t ticks = e_curr->ticks;
|
||||
// if (steps != 0) {
|
||||
// PROBE_1_TOGGLE;
|
||||
//}
|
||||
uint32_t last_entry;
|
||||
if (steps == 0) {
|
||||
q->bufferContainsSteps[fill_part_one ? 0 : 1] = false;
|
||||
// Perhaps the rmt performs look ahead
|
||||
ticks -= (PART_SIZE - 2) * 4 + 8;
|
||||
uint16_t ticks_l = ticks >> 1;
|
||||
uint16_t ticks_r = ticks - ticks_l;
|
||||
last_entry = ticks_l;
|
||||
last_entry <<= 16;
|
||||
last_entry |= ticks_r;
|
||||
*data++ = last_entry;
|
||||
for (uint8_t i = 1; i < PART_SIZE - 1; i++) {
|
||||
*data++ = 0x00020002;
|
||||
}
|
||||
last_entry = 0x00040004;
|
||||
} else {
|
||||
q->bufferContainsSteps[fill_part_one ? 0 : 1] = true;
|
||||
if (ticks == 0xffff) {
|
||||
// special treatment for this case, because an rmt entry can only cover up
|
||||
// to 0xfffe ticks every step must be minimum split into two rmt entries,
|
||||
// so at max PART/2 steps can be done.
|
||||
if (steps < PART_SIZE / 2) {
|
||||
for (uint8_t i = 1; i < steps; i++) {
|
||||
// steps-1 iterations
|
||||
*data++ = 0x40007fff | 0x8000;
|
||||
*data++ = 0x20002000;
|
||||
}
|
||||
*data++ = 0x40007fff | 0x8000;
|
||||
uint16_t delta = PART_SIZE - 2 * steps;
|
||||
delta <<= 5;
|
||||
*data++ = 0x20002000 - delta;
|
||||
// 2*(steps - 1) + 1 already stored => 2*steps - 1
|
||||
// and after this for loop one entry added => 2*steps
|
||||
for (uint8_t i = 2 * steps; i < PART_SIZE - 1; i++) {
|
||||
*data++ = 0x00100010;
|
||||
}
|
||||
last_entry = 0x00100010;
|
||||
steps = 0;
|
||||
} else {
|
||||
steps -= PART_SIZE / 2;
|
||||
for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) {
|
||||
*data++ = 0x40007fff | 0x8000;
|
||||
*data++ = 0x20002000;
|
||||
}
|
||||
*data++ = 0x40007fff | 0x8000;
|
||||
last_entry = 0x20002000;
|
||||
}
|
||||
} else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) {
|
||||
uint8_t steps_to_do = steps;
|
||||
if (steps > PART_SIZE) {
|
||||
steps_to_do /= 2;
|
||||
}
|
||||
|
||||
uint16_t ticks_l = ticks >> 1;
|
||||
uint16_t ticks_r = ticks - ticks_l;
|
||||
uint32_t rmt_entry = ticks_l;
|
||||
rmt_entry <<= 16;
|
||||
rmt_entry |= ticks_r | 0x8000; // with step
|
||||
for (uint8_t i = 1; i < steps_to_do; i++) {
|
||||
*data++ = rmt_entry;
|
||||
}
|
||||
uint32_t delta = (PART_SIZE - steps_to_do) * 4 + 8;
|
||||
delta <<= 16; // shift in upper 16bit
|
||||
*data++ = rmt_entry - delta;
|
||||
for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) {
|
||||
*data++ = 0x00020002;
|
||||
}
|
||||
last_entry = 0x00040004;
|
||||
steps -= steps_to_do;
|
||||
} else {
|
||||
// either >= 2*PART_SIZE or = PART_SIZE
|
||||
// every entry one step
|
||||
uint16_t ticks_l = ticks >> 1;
|
||||
uint16_t ticks_r = ticks - ticks_l;
|
||||
uint32_t rmt_entry = ticks_l;
|
||||
rmt_entry <<= 16;
|
||||
rmt_entry |= ticks_r | 0x8000; // with step
|
||||
for (uint8_t i = 0; i < PART_SIZE - 1; i++) {
|
||||
*data++ = rmt_entry;
|
||||
}
|
||||
last_entry = rmt_entry;
|
||||
steps -= PART_SIZE;
|
||||
}
|
||||
}
|
||||
// No tick lost mentioned for esp32c3
|
||||
// if (!fill_part_one) {
|
||||
// Note: When enabling the continuous transmission mode by setting
|
||||
// RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the
|
||||
// channel continuously, that is, from the first byte to the last one,
|
||||
// then from the first to the last again, and so on. In this mode, there
|
||||
// will be an idle level lasting one clk_div cycle between N and N+1
|
||||
// transmissions.
|
||||
// last_entry -= 1;
|
||||
// }
|
||||
*data = last_entry;
|
||||
|
||||
// Data is complete
|
||||
if (steps == 0) {
|
||||
// The command has been completed
|
||||
if (e_curr->repeat_entry == 0) {
|
||||
q->read_idx = rp + 1;
|
||||
}
|
||||
} else {
|
||||
e_curr->steps = steps;
|
||||
}
|
||||
}
|
||||
|
||||
#if !defined(RMT_CHANNEL_MEM) && !defined(HAVE_ESP32C3_RMT)
|
||||
#define RMT_LIMIT tx_lim
|
||||
#define RMT_FIFO apb_fifo_mask
|
||||
#else
|
||||
#define RMT_LIMIT limit
|
||||
#define RMT_FIFO fifo_mask
|
||||
#endif
|
||||
|
||||
// The threshold interrupts are happening in the "middle" of the previous entry.
|
||||
// Best strategy:
|
||||
// 1. enable threshold interrupt with PART_SIZE+2
|
||||
// 2. On every threshold interrupt toggle PART_SIZE and PART_SIZE+1
|
||||
// This way the first interrupt happens on the first entry of second half,
|
||||
// and the second interrupt on the first entry of the first half.
|
||||
// Afterwards alternating. This way the end interrupt is always "half buffer
|
||||
// away" from the threshold interrupt
|
||||
#ifdef TEST_MODE
|
||||
#define PROCESS_CHANNEL(ch) \
|
||||
if (mask & RMT_CH##ch##_TX_END_INT_ST) { \
|
||||
PROBE_1_TOGGLE; \
|
||||
} \
|
||||
if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \
|
||||
uint8_t old_limit = RMT.tx_lim[ch].RMT_LIMIT; \
|
||||
if (old_limit == PART_SIZE + 1) { \
|
||||
/* second half of buffer sent */ \
|
||||
PROBE_2_TOGGLE; \
|
||||
/* demonstrate modification of RAM */ \
|
||||
uint32_t *mem = FAS_RMT_MEM(ch); \
|
||||
mem[PART_SIZE] = 0x33ff33ff; \
|
||||
RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE; \
|
||||
} else { \
|
||||
/* first half of buffer sent */ \
|
||||
PROBE_3_TOGGLE; \
|
||||
RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \
|
||||
} \
|
||||
RMT.tx_conf[ch].conf_update = 1; \
|
||||
RMT.tx_conf[ch].conf_update = 0; \
|
||||
}
|
||||
#else
|
||||
#define PROCESS_CHANNEL(ch) \
|
||||
if (mask & RMT_CH##ch##_TX_END_INT_ST) { \
|
||||
StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \
|
||||
disable_rmt_interrupts(q->channel); \
|
||||
q->_isRunning = false; \
|
||||
PROBE_1_TOGGLE; \
|
||||
PROBE_2_TOGGLE; \
|
||||
PROBE_3_TOGGLE; \
|
||||
} \
|
||||
if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \
|
||||
uint8_t old_limit = RMT.tx_lim[ch].RMT_LIMIT; \
|
||||
StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \
|
||||
uint32_t *mem = FAS_RMT_MEM(ch); \
|
||||
if (old_limit == PART_SIZE + 1) { \
|
||||
/* second half of buffer sent */ \
|
||||
PROBE_2_TOGGLE; \
|
||||
apply_command(q, false, mem); \
|
||||
/* demonstrate modification of RAM */ \
|
||||
/*mem[PART_SIZE] = 0x33fff3ff; */ \
|
||||
RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE; \
|
||||
} else { \
|
||||
/* first half of buffer sent */ \
|
||||
PROBE_3_TOGGLE; \
|
||||
apply_command(q, true, mem); \
|
||||
RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \
|
||||
} \
|
||||
RMT.tx_conf[ch].conf_update = 1; \
|
||||
RMT.tx_conf[ch].conf_update = 0; \
|
||||
}
|
||||
#endif
|
||||
|
||||
static void IRAM_ATTR tx_intr_handler(void *arg) {
|
||||
uint32_t mask = RMT.int_st.val;
|
||||
RMT.int_clr.val = mask;
|
||||
PROCESS_CHANNEL(0);
|
||||
PROCESS_CHANNEL(1);
|
||||
#if QUEUES_RMT >= 4
|
||||
PROCESS_CHANNEL(2);
|
||||
PROCESS_CHANNEL(3);
|
||||
#endif
|
||||
#if QUEUES_RMT == 8
|
||||
PROCESS_CHANNEL(4);
|
||||
PROCESS_CHANNEL(5);
|
||||
PROCESS_CHANNEL(6);
|
||||
PROCESS_CHANNEL(7);
|
||||
#endif
|
||||
}
|
||||
|
||||
void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {
|
||||
#ifdef TEST_PROBE_1
|
||||
if (channel_num == 0) {
|
||||
pinMode(TEST_PROBE_1, OUTPUT);
|
||||
PROBE_1(HIGH);
|
||||
delay(10);
|
||||
PROBE_1(LOW);
|
||||
delay(5);
|
||||
PROBE_1(HIGH);
|
||||
delay(5);
|
||||
PROBE_1(LOW);
|
||||
delay(5);
|
||||
}
|
||||
#endif
|
||||
#ifdef TEST_PROBE_2
|
||||
pinMode(TEST_PROBE_2, OUTPUT);
|
||||
PROBE_2(LOW);
|
||||
#endif
|
||||
#ifdef TEST_PROBE_3
|
||||
pinMode(TEST_PROBE_3, OUTPUT);
|
||||
PROBE_3(LOW);
|
||||
#endif
|
||||
|
||||
_initVars();
|
||||
_step_pin = step_pin;
|
||||
// digitalWrite(step_pin, LOW);
|
||||
// pinMode(step_pin, OUTPUT);
|
||||
|
||||
channel = (rmt_channel_t)channel_num;
|
||||
|
||||
if (channel_num == 0) {
|
||||
periph_module_enable(PERIPH_RMT_MODULE);
|
||||
}
|
||||
// APB_CLOCK=80 MHz
|
||||
// CLK_DIV = APB_CLOCK/5 = 16 MHz
|
||||
//
|
||||
// Relation 1 in esp32c3 technical reference:
|
||||
// 3 * T_APB + 5 * T_RMT_CLK < period * T_CLK_DIV
|
||||
// => 8 * T_APB < period * T_APB*5
|
||||
// => period > 8/5
|
||||
// => period >= 2
|
||||
//
|
||||
// Relation 2 in esp32c3 technical reference before end marker:
|
||||
// 6 * T_APB + 12 * T_RMT_CLK < period * T_CLK_DIV
|
||||
// => 18 * T_APB < period * T_APB*5
|
||||
// => period > 18/5
|
||||
// => period >= 4
|
||||
//
|
||||
disable_rmt_interrupts(channel);
|
||||
rmt_set_source_clk(channel, RMT_BASECLK_APB);
|
||||
rmt_set_clk_div(channel, 5);
|
||||
rmt_set_mem_block_num(channel, 1);
|
||||
rmt_set_tx_carrier(channel, false, 0, 0, RMT_CARRIER_LEVEL_LOW);
|
||||
rmt_tx_stop(channel);
|
||||
rmt_tx_memory_reset(channel);
|
||||
// rmt_rx_stop(channel); TX only channel !
|
||||
// rmt_tx_memory_reset is not defined in arduino V340 and based on test
|
||||
// result not needed rmt_tx_memory_reset(channel);
|
||||
if (channel_num == 0) {
|
||||
rmt_isr_register(tx_intr_handler, NULL,
|
||||
ESP_INTR_FLAG_SHARED | ESP_INTR_FLAG_IRAM, NULL);
|
||||
RMT.sys_conf.fifo_mask = 1; // disable fifo mode
|
||||
}
|
||||
|
||||
_isRunning = false;
|
||||
_rmtStopped = true;
|
||||
|
||||
connect_rmt();
|
||||
|
||||
#ifdef TEST_MODE
|
||||
if (channel == 0) {
|
||||
RMT.tx_conf[channel].mem_rd_rst = 1;
|
||||
RMT.tx_conf[channel].mem_rd_rst = 0;
|
||||
uint32_t *mem = FAS_RMT_MEM(channel);
|
||||
// Fill the buffer with a significant pattern for debugging
|
||||
for (uint8_t i = 0; i < PART_SIZE; i++) {
|
||||
*mem++ = 0x7fffffff;
|
||||
}
|
||||
for (uint8_t i = PART_SIZE; i < 2 * PART_SIZE; i++) {
|
||||
*mem++ = 0x3fffdfff;
|
||||
}
|
||||
// without end marker it does not loop
|
||||
*mem++ = 0;
|
||||
*mem++ = 0;
|
||||
|
||||
// conti mode is accepted with the conf_update 1 strobe
|
||||
RMT.tx_conf[channel].tx_conti_mode = 1;
|
||||
RMT.tx_conf[channel].conf_update = 1;
|
||||
RMT.tx_conf[channel].conf_update = 0;
|
||||
RMT.tx_conf[channel].mem_tx_wrap_en = 0;
|
||||
RMT.tx_conf[channel].conf_update = 1;
|
||||
RMT.tx_conf[channel].conf_update = 0;
|
||||
enable_rmt_interrupts(channel);
|
||||
// tx_start does not need conf_update
|
||||
PROBE_1_TOGGLE; // end interrupt will toggle again PROBE_1
|
||||
RMT.tx_conf[channel].tx_start = 1;
|
||||
|
||||
delay(1000);
|
||||
if (false) {
|
||||
mem--;
|
||||
mem--;
|
||||
// destroy end marker => no end interrupt, no repeat
|
||||
*mem++ = 0x00010001;
|
||||
*mem = 0x00010001;
|
||||
}
|
||||
if (true) {
|
||||
// just clear conti mode => causes end interrupt, no repeat
|
||||
RMT.tx_conf[channel].tx_conti_mode = 0;
|
||||
RMT.tx_conf[channel].conf_update = 1;
|
||||
RMT.tx_conf[channel].conf_update = 0;
|
||||
}
|
||||
delay(1000);
|
||||
// actually no need to enable/disable interrupts.
|
||||
// and this seems to avoid some pitfalls
|
||||
|
||||
// This runs the RMT buffer once
|
||||
RMT.tx_conf[channel].tx_conti_mode = 0;
|
||||
RMT.tx_conf[channel].conf_update = 1;
|
||||
RMT.tx_conf[channel].conf_update = 0;
|
||||
RMT.tx_conf[channel].tx_start = 1;
|
||||
while (true) {
|
||||
delay(1000);
|
||||
PROBE_1_TOGGLE;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void StepperQueue::connect_rmt() {
|
||||
RMT.tx_conf[channel].idle_out_lv = 0;
|
||||
RMT.tx_conf[channel].idle_out_en = 1;
|
||||
RMT.tx_conf[channel].conf_update = 1;
|
||||
RMT.tx_conf[channel].conf_update = 0;
|
||||
// RMT.tx_conf[channel].mem_tx_wrap_en = 0;
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)_step_pin);
|
||||
#else
|
||||
rmt_set_gpio(channel, RMT_MODE_TX, (gpio_num_t)_step_pin, false);
|
||||
#endif
|
||||
|
||||
#ifdef TEST_MODE
|
||||
// here gpio is 0
|
||||
delay(1);
|
||||
PROBE_3_TOGGLE;
|
||||
RMT.tx_conf[channel].idle_out_lv = 1;
|
||||
RMT.tx_conf[channel].conf_update = 1;
|
||||
RMT.tx_conf[channel].conf_update = 0;
|
||||
// here gpio is 1
|
||||
delay(2);
|
||||
PROBE_3_TOGGLE;
|
||||
RMT.tx_conf[channel].idle_out_lv = 0;
|
||||
RMT.tx_conf[channel].conf_update = 1;
|
||||
RMT.tx_conf[channel].conf_update = 0;
|
||||
// here gpio is 0
|
||||
delay(2);
|
||||
PROBE_3_TOGGLE;
|
||||
RMT.tx_conf[channel].idle_out_en = 0;
|
||||
RMT.tx_conf[channel].conf_update = 1;
|
||||
RMT.tx_conf[channel].conf_update = 0;
|
||||
// here gpio is 0
|
||||
delay(2);
|
||||
PROBE_3_TOGGLE;
|
||||
RMT.tx_conf[channel].idle_out_lv = 1;
|
||||
RMT.tx_conf[channel].idle_out_en = 1;
|
||||
RMT.tx_conf[channel].conf_update = 1;
|
||||
RMT.tx_conf[channel].conf_update = 0;
|
||||
// here gpio is 1
|
||||
delay(2);
|
||||
PROBE_3_TOGGLE;
|
||||
RMT.tx_conf[channel].idle_out_lv = 0;
|
||||
RMT.tx_conf[channel].conf_update = 1;
|
||||
RMT.tx_conf[channel].conf_update = 0;
|
||||
// here gpio is 0
|
||||
delay(2);
|
||||
PROBE_3_TOGGLE;
|
||||
#endif
|
||||
}
|
||||
|
||||
void StepperQueue::disconnect_rmt() {
|
||||
disable_rmt_interrupts(channel);
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
// rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)-1);
|
||||
#else
|
||||
// rmt_set_gpio(channel, RMT_MODE_TX, GPIO_NUM_NC, false);
|
||||
#endif
|
||||
RMT.tx_conf[channel].idle_out_en = 0;
|
||||
RMT.tx_conf[channel].conf_update = 1;
|
||||
RMT.tx_conf[channel].conf_update = 0;
|
||||
}
|
||||
|
||||
void StepperQueue::startQueue_rmt() {
|
||||
#ifdef TRACE
|
||||
USBSerial.println("START");
|
||||
#endif
|
||||
#ifdef TEST_PROBE_2
|
||||
PROBE_2(LOW);
|
||||
#endif
|
||||
#ifdef TEST_PROBE_3
|
||||
PROBE_3(LOW);
|
||||
#endif
|
||||
#if defined(TEST_PROBE_1) && defined(TEST_PROBE_2) && defined(TEST_PROBE_3)
|
||||
delay(1);
|
||||
PROBE_1_TOGGLE;
|
||||
delay(1);
|
||||
PROBE_1_TOGGLE;
|
||||
delay(1);
|
||||
PROBE_1_TOGGLE;
|
||||
delay(1);
|
||||
PROBE_2_TOGGLE;
|
||||
delay(2);
|
||||
PROBE_2_TOGGLE;
|
||||
delay(2);
|
||||
PROBE_3_TOGGLE;
|
||||
delay(3);
|
||||
PROBE_3_TOGGLE;
|
||||
delay(3);
|
||||
#endif
|
||||
rmt_tx_stop(channel);
|
||||
// rmt_rx_stop(channel);
|
||||
RMT.tx_conf[channel].mem_rd_rst = 1;
|
||||
RMT.tx_conf[channel].mem_rd_rst = 0;
|
||||
uint32_t *mem = FAS_RMT_MEM(channel);
|
||||
// #define TRACE
|
||||
#ifdef TRACE
|
||||
// Fill the buffer with a significant pattern for debugging
|
||||
// Keep it for now
|
||||
for (uint8_t i = 0; i < 2 * PART_SIZE; i += 2) {
|
||||
// mem[i] = 0x0fff8fff;
|
||||
// mem[i + 1] = 0x7fff8fff;
|
||||
}
|
||||
#endif
|
||||
// Write end marker
|
||||
mem[2 * PART_SIZE] = 0;
|
||||
mem[2 * PART_SIZE + 1] = 0;
|
||||
_isRunning = true;
|
||||
_rmtStopped = false;
|
||||
disable_rmt_interrupts(channel);
|
||||
RMT.tx_conf[channel].mem_tx_wrap_en = 0;
|
||||
RMT.tx_conf[channel].conf_update = 1;
|
||||
RMT.tx_conf[channel].conf_update = 0;
|
||||
|
||||
#ifdef TRACE
|
||||
USBSerial.print("Queue:");
|
||||
USBSerial.print(read_idx);
|
||||
USBSerial.print('/');
|
||||
USBSerial.println(next_write_idx);
|
||||
for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) {
|
||||
USBSerial.print(i);
|
||||
USBSerial.print(' ');
|
||||
USBSerial.println(mem[i], HEX);
|
||||
}
|
||||
#endif
|
||||
|
||||
// set dirpin toggle here
|
||||
uint8_t rp = read_idx;
|
||||
if (rp == next_write_idx) {
|
||||
// nothing to do ?
|
||||
// Should not happen, so bail
|
||||
return;
|
||||
}
|
||||
if (entry[rp & QUEUE_LEN_MASK].toggle_dir) {
|
||||
LL_TOGGLE_PIN(dirPin);
|
||||
entry[rp & QUEUE_LEN_MASK].toggle_dir = false;
|
||||
}
|
||||
|
||||
bufferContainsSteps[0] = true;
|
||||
bufferContainsSteps[1] = true;
|
||||
apply_command(this, true, mem);
|
||||
|
||||
#ifdef TRACE
|
||||
USBSerial.print(RMT.tx_conf[channel].val, BIN);
|
||||
USBSerial.println(' ');
|
||||
USBSerial.print(RMT.tx_conf[channel].mem_tx_wrap_en);
|
||||
USBSerial.println(' ');
|
||||
for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) {
|
||||
USBSerial.print(i);
|
||||
USBSerial.print(' ');
|
||||
USBSerial.println(mem[i], HEX);
|
||||
}
|
||||
if (!isRunning()) {
|
||||
USBSerial.println("STOPPED 1");
|
||||
}
|
||||
#endif
|
||||
|
||||
apply_command(this, false, mem);
|
||||
|
||||
#ifdef TRACE
|
||||
USBSerial.print(RMT.tx_conf[channel].val, BIN);
|
||||
USBSerial.print(' ');
|
||||
USBSerial.print(RMT.tx_conf[channel].mem_tx_wrap_en);
|
||||
USBSerial.println(' ');
|
||||
|
||||
for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) {
|
||||
USBSerial.print(i);
|
||||
USBSerial.print(' ');
|
||||
USBSerial.println(mem[i], HEX);
|
||||
}
|
||||
#endif
|
||||
enable_rmt_interrupts(channel);
|
||||
|
||||
#ifdef TRACE
|
||||
USBSerial.print("Interrupt enable:");
|
||||
USBSerial.println(RMT.int_ena.val, BIN);
|
||||
USBSerial.print("Interrupt status:");
|
||||
USBSerial.println(RMT.int_st.val, BIN);
|
||||
USBSerial.print("Threshold: 0x");
|
||||
USBSerial.println(RMT.tx_lim[channel].val, HEX);
|
||||
#endif
|
||||
|
||||
// This starts the rmt module
|
||||
RMT.tx_conf[channel].tx_conti_mode = 1;
|
||||
RMT.tx_conf[channel].conf_update = 1;
|
||||
RMT.tx_conf[channel].conf_update = 0;
|
||||
RMT.tx_conf[channel].mem_tx_wrap_en = 0;
|
||||
RMT.tx_conf[channel].conf_update = 1;
|
||||
RMT.tx_conf[channel].conf_update = 0;
|
||||
|
||||
PROBE_1_TOGGLE;
|
||||
RMT.tx_conf[channel].tx_start = 1;
|
||||
}
|
||||
void StepperQueue::forceStop_rmt() {
|
||||
stop_rmt(true);
|
||||
|
||||
// and empty the buffer
|
||||
read_idx = next_write_idx;
|
||||
}
|
||||
bool StepperQueue::isReadyForCommands_rmt() {
|
||||
if (_isRunning) {
|
||||
return !_rmtStopped;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
uint16_t StepperQueue::_getPerformedPulses_rmt() { return 0; }
|
||||
#endif
|
||||
652
src/StepperISR_idf4_esp32s3_rmt.cpp
Normal file
652
src/StepperISR_idf4_esp32s3_rmt.cpp
Normal file
@@ -0,0 +1,652 @@
|
||||
#include "StepperISR.h"
|
||||
#if defined(HAVE_ESP32S3_RMT) && (ESP_IDF_VERSION_MAJOR == 4)
|
||||
|
||||
// #define TEST_MODE
|
||||
// #define TRACE
|
||||
|
||||
#include "fas_arch/test_probe.h"
|
||||
|
||||
// The following concept is in use:
|
||||
//
|
||||
// The rmt buffer is split into two parts.
|
||||
// Each part will hold one command (or part of).
|
||||
// After the two parts an end marker is placed.
|
||||
//
|
||||
// Of these 32 bits, the low 16-bit entry is sent first and the high entry
|
||||
// second.
|
||||
// Every 16 bit entry defines with MSB the output level and the lower 15 bits
|
||||
// the ticks.
|
||||
//
|
||||
// Important difference of esp32s3 (compared to esp32):
|
||||
// - configuration updates need an conf_update strobe
|
||||
// (apparently the manual is not correct by mentioning to set conf_update
|
||||
// first)
|
||||
// - if the end marker is hit in continuous mode, there is no end interrupt
|
||||
// - there is no tick lost with the end marker
|
||||
// - minimum periods as per relation 1 and 2 to be adhered to
|
||||
//
|
||||
//
|
||||
#ifdef HAVE_ESP32S3_RMT
|
||||
#define PART_SIZE 23
|
||||
#define RMT_MEM_SIZE 48
|
||||
#else
|
||||
#error
|
||||
#define PART_SIZE 31
|
||||
#define RMT_MEM_SIZE 64
|
||||
#endif
|
||||
|
||||
// In order to avoid threshold/end interrupt on end, add one
|
||||
#define enable_rmt_interrupts(channel) \
|
||||
{ \
|
||||
RMT.chn_tx_lim[channel].RMT_LIMIT = PART_SIZE + 2; \
|
||||
RMT.chnconf0[channel].conf_update_n = 1; \
|
||||
RMT.chnconf0[channel].conf_update_n = 0; \
|
||||
RMT.int_clr.val |= 0x101 << channel; \
|
||||
RMT.int_ena.val |= 0x101 << channel; \
|
||||
}
|
||||
#define disable_rmt_interrupts(channel) \
|
||||
{ RMT.int_ena.val &= ~(0x101 << channel); }
|
||||
|
||||
void IRAM_ATTR StepperQueue::stop_rmt(bool both) {
|
||||
// We are stopping the rmt by letting it run into the end at high speed.
|
||||
//
|
||||
// disable the interrupts
|
||||
// rmt_set_tx_intr_en(channel, false);
|
||||
// rmt_set_tx_thr_intr_en(channel, false, 0);
|
||||
|
||||
// stop esp32 rmt, by let it hit the end
|
||||
RMT.chnconf0[channel].tx_conti_mode_n = 0;
|
||||
RMT.chnconf0[channel].conf_update_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 0;
|
||||
|
||||
// replace second part of buffer with pauses
|
||||
uint32_t *data = FAS_RMT_MEM(channel);
|
||||
uint8_t start = both ? 0 : PART_SIZE;
|
||||
data = &data[start];
|
||||
for (uint8_t i = start; i < 2 * PART_SIZE; i++) {
|
||||
// two pauses à n ticks to achieve MIN_CMD_TICKS
|
||||
*data++ = 0x00010001 * ((MIN_CMD_TICKS + 61) / 62);
|
||||
}
|
||||
*data = 0;
|
||||
|
||||
// as the rmt is not running anymore, mark it as stopped
|
||||
_rmtStopped = true;
|
||||
}
|
||||
|
||||
static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one,
|
||||
uint32_t *data) {
|
||||
if (!fill_part_one) {
|
||||
data += PART_SIZE;
|
||||
}
|
||||
uint8_t rp = q->read_idx;
|
||||
if (rp == q->next_write_idx) {
|
||||
// no command in queue
|
||||
if (fill_part_one) {
|
||||
q->bufferContainsSteps[0] = false;
|
||||
for (uint8_t i = 0; i < PART_SIZE; i++) {
|
||||
// make a pause with approx. 1ms
|
||||
// 258 ticks * 2 * 31 = 15996 @ 16MHz
|
||||
// 347 ticks * 2 * 23 = 15962 @ 16MHz
|
||||
*data++ = 0x010001 * (16000 / 2 / PART_SIZE);
|
||||
}
|
||||
} else {
|
||||
q->stop_rmt(false);
|
||||
}
|
||||
return;
|
||||
}
|
||||
// Process command
|
||||
struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK];
|
||||
|
||||
if (e_curr->toggle_dir) {
|
||||
// the command requests dir pin toggle
|
||||
// This is ok only, if the ongoing command does not contain steps
|
||||
if (q->bufferContainsSteps[fill_part_one ? 1 : 0]) {
|
||||
// So we need a pause. change the finished read entry into a pause
|
||||
q->bufferContainsSteps[fill_part_one ? 0 : 1] = false;
|
||||
for (uint8_t i = 0; i < PART_SIZE; i++) {
|
||||
// two pauses à n ticks to achieve MIN_CMD_TICKS
|
||||
*data++ = 0x00010001 *
|
||||
((MIN_CMD_TICKS + 2 * PART_SIZE - 1) / (2 * PART_SIZE));
|
||||
}
|
||||
return;
|
||||
}
|
||||
// The ongoing command does not contain steps, so change dir here should be
|
||||
// ok
|
||||
LL_TOGGLE_PIN(q->dirPin);
|
||||
// and delete the request
|
||||
e_curr->toggle_dir = 0;
|
||||
}
|
||||
|
||||
uint8_t steps = e_curr->steps;
|
||||
uint16_t ticks = e_curr->ticks;
|
||||
// if (steps != 0) {
|
||||
// PROBE_1_TOGGLE;
|
||||
//}
|
||||
uint32_t last_entry;
|
||||
if (steps == 0) {
|
||||
q->bufferContainsSteps[fill_part_one ? 0 : 1] = false;
|
||||
// Perhaps the rmt performs look ahead
|
||||
ticks -= (PART_SIZE - 2) * 4 + 8;
|
||||
uint16_t ticks_l = ticks >> 1;
|
||||
uint16_t ticks_r = ticks - ticks_l;
|
||||
last_entry = ticks_l;
|
||||
last_entry <<= 16;
|
||||
last_entry |= ticks_r;
|
||||
*data++ = last_entry;
|
||||
for (uint8_t i = 1; i < PART_SIZE - 1; i++) {
|
||||
*data++ = 0x00020002;
|
||||
}
|
||||
last_entry = 0x00040004;
|
||||
} else {
|
||||
q->bufferContainsSteps[fill_part_one ? 0 : 1] = true;
|
||||
if (ticks == 0xffff) {
|
||||
// special treatment for this case, because an rmt entry can only cover up
|
||||
// to 0xfffe ticks every step must be minimum split into two rmt entries,
|
||||
// so at max PART/2 steps can be done.
|
||||
if (steps < PART_SIZE / 2) {
|
||||
for (uint8_t i = 1; i < steps; i++) {
|
||||
// steps-1 iterations
|
||||
*data++ = 0x40007fff | 0x8000;
|
||||
*data++ = 0x20002000;
|
||||
}
|
||||
*data++ = 0x40007fff | 0x8000;
|
||||
uint16_t delta = PART_SIZE - 2 * steps;
|
||||
delta <<= 5;
|
||||
*data++ = 0x20002000 - delta;
|
||||
// 2*(steps - 1) + 1 already stored => 2*steps - 1
|
||||
// and after this for loop one entry added => 2*steps
|
||||
for (uint8_t i = 2 * steps; i < PART_SIZE - 1; i++) {
|
||||
*data++ = 0x00100010;
|
||||
}
|
||||
last_entry = 0x00100010;
|
||||
steps = 0;
|
||||
} else {
|
||||
steps -= PART_SIZE / 2;
|
||||
for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) {
|
||||
*data++ = 0x40007fff | 0x8000;
|
||||
*data++ = 0x20002000;
|
||||
}
|
||||
*data++ = 0x40007fff | 0x8000;
|
||||
last_entry = 0x20002000;
|
||||
}
|
||||
} else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) {
|
||||
uint8_t steps_to_do = steps;
|
||||
if (steps > PART_SIZE) {
|
||||
steps_to_do /= 2;
|
||||
}
|
||||
|
||||
uint16_t ticks_l = ticks >> 1;
|
||||
uint16_t ticks_r = ticks - ticks_l;
|
||||
uint32_t rmt_entry = ticks_l;
|
||||
rmt_entry <<= 16;
|
||||
rmt_entry |= ticks_r | 0x8000; // with step
|
||||
for (uint8_t i = 1; i < steps_to_do; i++) {
|
||||
*data++ = rmt_entry;
|
||||
}
|
||||
uint32_t delta = (PART_SIZE - steps_to_do) * 4 + 8;
|
||||
delta <<= 16; // shift in upper 16bit
|
||||
*data++ = rmt_entry - delta;
|
||||
for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) {
|
||||
*data++ = 0x00020002;
|
||||
}
|
||||
last_entry = 0x00040004;
|
||||
steps -= steps_to_do;
|
||||
} else {
|
||||
// either >= 2*PART_SIZE or = PART_SIZE
|
||||
// every entry one step
|
||||
uint16_t ticks_l = ticks >> 1;
|
||||
uint16_t ticks_r = ticks - ticks_l;
|
||||
uint32_t rmt_entry = ticks_l;
|
||||
rmt_entry <<= 16;
|
||||
rmt_entry |= ticks_r | 0x8000; // with step
|
||||
for (uint8_t i = 0; i < PART_SIZE - 1; i++) {
|
||||
*data++ = rmt_entry;
|
||||
}
|
||||
last_entry = rmt_entry;
|
||||
steps -= PART_SIZE;
|
||||
}
|
||||
}
|
||||
// No tick lost mentioned for esp32s3
|
||||
// if (!fill_part_one) {
|
||||
// Note: When enabling the continuous transmission mode by setting
|
||||
// RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the
|
||||
// channel continuously, that is, from the first byte to the last one,
|
||||
// then from the first to the last again, and so on. In this mode, there
|
||||
// will be an idle level lasting one clk_div cycle between N and N+1
|
||||
// transmissions.
|
||||
// last_entry -= 1;
|
||||
// }
|
||||
*data = last_entry;
|
||||
|
||||
// Data is complete
|
||||
if (steps == 0) {
|
||||
// The command has been completed
|
||||
if (e_curr->repeat_entry == 0) {
|
||||
q->read_idx = rp + 1;
|
||||
}
|
||||
} else {
|
||||
e_curr->steps = steps;
|
||||
}
|
||||
}
|
||||
|
||||
#if !defined(RMT_CHANNEL_MEM) && !defined(HAVE_ESP32S3_RMT)
|
||||
#define RMT_LIMIT tx_lim
|
||||
#define RMT_FIFO apb_fifo_mask
|
||||
#else
|
||||
#define RMT_LIMIT tx_lim_chn
|
||||
#define RMT_FIFO apb_fifo_mask
|
||||
#endif
|
||||
|
||||
// The threshold interrupts are happening in the "middle" of the previous entry.
|
||||
// Best strategy:
|
||||
// 1. enable threshold interrupt with PART_SIZE+2
|
||||
// 2. On every threshold interrupt toggle PART_SIZE and PART_SIZE+1
|
||||
// This way the first interrupt happens on the first entry of second half,
|
||||
// and the second interrupt on the first entry of the first half.
|
||||
// Afterwards alternating. This way the end interrupt is always "half buffer
|
||||
// away" from the threshold interrupt
|
||||
#ifdef TEST_MODE
|
||||
#define PROCESS_CHANNEL(ch) \
|
||||
if (mask & RMT_CH##ch##_TX_END_INT_ST) { \
|
||||
PROBE_1_TOGGLE; \
|
||||
} \
|
||||
if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \
|
||||
uint8_t old_limit = RMT.chn_tx_lim[ch].RMT_LIMIT; \
|
||||
if (old_limit == PART_SIZE + 1) { \
|
||||
/* second half of buffer sent */ \
|
||||
PROBE_2_TOGGLE; \
|
||||
/* demonstrate modification of RAM */ \
|
||||
uint32_t *mem = FAS_RMT_MEM(ch); \
|
||||
mem[PART_SIZE] = 0x33ff33ff; \
|
||||
RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE; \
|
||||
} else { \
|
||||
/* first half of buffer sent */ \
|
||||
PROBE_3_TOGGLE; \
|
||||
RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \
|
||||
} \
|
||||
RMT.chnconf0[ch].conf_update_n = 1; \
|
||||
RMT.chnconf0[ch].conf_update_n = 0; \
|
||||
}
|
||||
#else
|
||||
#define PROCESS_CHANNEL(ch) \
|
||||
if (mask & RMT_CH##ch##_TX_END_INT_ST) { \
|
||||
StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \
|
||||
disable_rmt_interrupts(q->channel); \
|
||||
q->_isRunning = false; \
|
||||
PROBE_1_TOGGLE; \
|
||||
PROBE_2_TOGGLE; \
|
||||
PROBE_3_TOGGLE; \
|
||||
} \
|
||||
if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \
|
||||
uint8_t old_limit = RMT.chn_tx_lim[ch].RMT_LIMIT; \
|
||||
StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \
|
||||
uint32_t *mem = FAS_RMT_MEM(ch); \
|
||||
if (old_limit == PART_SIZE + 1) { \
|
||||
/* second half of buffer sent */ \
|
||||
PROBE_2_TOGGLE; \
|
||||
apply_command(q, false, mem); \
|
||||
/* demonstrate modification of RAM */ \
|
||||
/*mem[PART_SIZE] = 0x33fff3ff; */ \
|
||||
RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE; \
|
||||
} else { \
|
||||
/* first half of buffer sent */ \
|
||||
PROBE_3_TOGGLE; \
|
||||
apply_command(q, true, mem); \
|
||||
RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \
|
||||
} \
|
||||
RMT.chnconf0[ch].conf_update_n = 1; \
|
||||
RMT.chnconf0[ch].conf_update_n = 0; \
|
||||
}
|
||||
#endif
|
||||
|
||||
static void IRAM_ATTR tx_intr_handler(void *arg) {
|
||||
uint32_t mask = RMT.int_st.val;
|
||||
RMT.int_clr.val = mask;
|
||||
PROCESS_CHANNEL(0);
|
||||
PROCESS_CHANNEL(1);
|
||||
#if QUEUES_RMT >= 4
|
||||
PROCESS_CHANNEL(2);
|
||||
PROCESS_CHANNEL(3);
|
||||
#endif
|
||||
#if QUEUES_RMT == 8
|
||||
PROCESS_CHANNEL(4);
|
||||
PROCESS_CHANNEL(5);
|
||||
PROCESS_CHANNEL(6);
|
||||
PROCESS_CHANNEL(7);
|
||||
#endif
|
||||
}
|
||||
|
||||
void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {
|
||||
#ifdef TEST_PROBE_1
|
||||
if (channel_num == 0) {
|
||||
pinMode(TEST_PROBE_1, OUTPUT);
|
||||
PROBE_1(HIGH);
|
||||
delay(10);
|
||||
PROBE_1(LOW);
|
||||
delay(5);
|
||||
PROBE_1(HIGH);
|
||||
delay(5);
|
||||
PROBE_1(LOW);
|
||||
delay(5);
|
||||
}
|
||||
#endif
|
||||
#ifdef TEST_PROBE_2
|
||||
pinMode(TEST_PROBE_2, OUTPUT);
|
||||
PROBE_2(LOW);
|
||||
#endif
|
||||
#ifdef TEST_PROBE_3
|
||||
pinMode(TEST_PROBE_3, OUTPUT);
|
||||
PROBE_3(LOW);
|
||||
#endif
|
||||
|
||||
_initVars();
|
||||
_step_pin = step_pin;
|
||||
// digitalWrite(step_pin, LOW);
|
||||
// pinMode(step_pin, OUTPUT);
|
||||
|
||||
channel = (rmt_channel_t)channel_num;
|
||||
|
||||
if (channel_num == 0) {
|
||||
periph_module_enable(PERIPH_RMT_MODULE);
|
||||
}
|
||||
// APB_CLOCK=80 MHz
|
||||
// CLK_DIV = APB_CLOCK/5 = 16 MHz
|
||||
//
|
||||
// Relation 1 in esp32s3 technical reference:
|
||||
// 3 * T_APB + 5 * T_RMT_CLK < period * T_CLK_DIV
|
||||
// => 8 * T_APB < period * T_APB*5
|
||||
// => period > 8/5
|
||||
// => period >= 2
|
||||
//
|
||||
// Relation 2 in esp32s3 technical reference before end marker:
|
||||
// 6 * T_APB + 12 * T_RMT_CLK < period * T_CLK_DIV
|
||||
// => 18 * T_APB < period * T_APB*5
|
||||
// => period > 18/5
|
||||
// => period >= 4
|
||||
//
|
||||
disable_rmt_interrupts(channel);
|
||||
rmt_set_source_clk(channel, RMT_BASECLK_APB);
|
||||
rmt_set_clk_div(channel, 5);
|
||||
rmt_set_mem_block_num(channel, 1);
|
||||
rmt_set_tx_carrier(channel, false, 0, 0, RMT_CARRIER_LEVEL_LOW);
|
||||
rmt_tx_stop(channel);
|
||||
rmt_tx_memory_reset(channel);
|
||||
// rmt_rx_stop(channel); TX only channel !
|
||||
// rmt_tx_memory_reset is not defined in arduino V340 and based on test
|
||||
// result not needed rmt_tx_memory_reset(channel);
|
||||
if (channel_num == 0) {
|
||||
rmt_isr_register(tx_intr_handler, NULL,
|
||||
ESP_INTR_FLAG_SHARED | ESP_INTR_FLAG_IRAM, NULL);
|
||||
RMT.sys_conf.apb_fifo_mask = 1; // disable fifo mode
|
||||
}
|
||||
|
||||
_isRunning = false;
|
||||
_rmtStopped = true;
|
||||
|
||||
connect_rmt();
|
||||
|
||||
#ifdef TEST_MODE
|
||||
if (channel == 0) {
|
||||
RMT.chnconf0[channel].mem_rd_rst_n = 1;
|
||||
RMT.chnconf0[channel].mem_rd_rst_n = 0;
|
||||
uint32_t *mem = FAS_RMT_MEM(channel);
|
||||
// Fill the buffer with a significant pattern for debugging
|
||||
for (uint8_t i = 0; i < PART_SIZE; i++) {
|
||||
*mem++ = 0x7fffffff;
|
||||
}
|
||||
for (uint8_t i = PART_SIZE; i < 2 * PART_SIZE; i++) {
|
||||
*mem++ = 0x3fffdfff;
|
||||
}
|
||||
// without end marker it does not loop
|
||||
*mem++ = 0;
|
||||
*mem++ = 0;
|
||||
|
||||
// conti mode is accepted with the conf_update 1 strobe
|
||||
RMT.chnconf0[channel].tx_conti_mode_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 0;
|
||||
RMT.chnconf0[channel].mem_tx_wrap_en_n = 0;
|
||||
RMT.chnconf0[channel].conf_update_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 0;
|
||||
enable_rmt_interrupts(channel);
|
||||
// tx_start does not need conf_update
|
||||
PROBE_1_TOGGLE; // end interrupt will toggle again PROBE_1
|
||||
RMT.chnconf0[channel].tx_start_n = 1;
|
||||
|
||||
delay(1000);
|
||||
if (false) {
|
||||
mem--;
|
||||
mem--;
|
||||
// destroy end marker => no end interrupt, no repeat
|
||||
*mem++ = 0x00010001;
|
||||
*mem = 0x00010001;
|
||||
}
|
||||
if (true) {
|
||||
// just clear conti mode => causes end interrupt, no repeat
|
||||
RMT.chnconf0[channel].tx_conti_mode_n = 0;
|
||||
RMT.chnconf0[channel].conf_update_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 0;
|
||||
}
|
||||
delay(1000);
|
||||
// actually no need to enable/disable interrupts.
|
||||
// and this seems to avoid some pitfalls
|
||||
|
||||
// This runs the RMT buffer once
|
||||
RMT.chnconf0[channel].tx_conti_mode_n = 0;
|
||||
RMT.chnconf0[channel].conf_update_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 0;
|
||||
RMT.chnconf0[channel].tx_start_n = 1;
|
||||
while (true) {
|
||||
delay(1000);
|
||||
PROBE_1_TOGGLE;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void StepperQueue::connect_rmt() {
|
||||
RMT.chnconf0[channel].idle_out_lv_n = 0;
|
||||
RMT.chnconf0[channel].idle_out_en_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 0;
|
||||
// RMT.tx_conf[channel].mem_tx_wrap_en = 0;
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)_step_pin);
|
||||
#else
|
||||
rmt_set_gpio(channel, RMT_MODE_TX, (gpio_num_t)_step_pin, false);
|
||||
#endif
|
||||
|
||||
#ifdef TEST_MODE
|
||||
// here gpio is 0
|
||||
delay(1);
|
||||
PROBE_3_TOGGLE;
|
||||
RMT.tx_conf[channel].idle_out_lv_n = 1;
|
||||
RMT.tx_conf[channel].conf_update_n = 1;
|
||||
RMT.tx_conf[channel].conf_update_n = 0;
|
||||
// here gpio is 1
|
||||
delay(2);
|
||||
PROBE_3_TOGGLE;
|
||||
RMT.chnconf0[channel].idle_out_lv_n = 0;
|
||||
RMT.chnconf0[channel].conf_update_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 0;
|
||||
// here gpio is 0
|
||||
delay(2);
|
||||
PROBE_3_TOGGLE;
|
||||
RMT.chnconf0[channel].idle_out_en_n = 0;
|
||||
RMT.chnconf0[channel].conf_update_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 0;
|
||||
// here gpio is 0
|
||||
delay(2);
|
||||
PROBE_3_TOGGLE;
|
||||
RMT.chnconf0[channel].idle_out_lv_n = 1;
|
||||
RMT.chnconf0[channel].idle_out_en_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 0;
|
||||
// here gpio is 1
|
||||
delay(2);
|
||||
PROBE_3_TOGGLE;
|
||||
RMT.chnconf0[channel].idle_out_lv_n = 0;
|
||||
RMT.chnconf0[channel].conf_update_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 0;
|
||||
// here gpio is 0
|
||||
delay(2);
|
||||
PROBE_3_TOGGLE;
|
||||
#endif
|
||||
}
|
||||
|
||||
void StepperQueue::disconnect_rmt() {
|
||||
disable_rmt_interrupts(channel);
|
||||
#ifndef __ESP32_IDF_V44__
|
||||
// rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)-1);
|
||||
#else
|
||||
// rmt_set_gpio(channel, RMT_MODE_TX, GPIO_NUM_NC, false);
|
||||
#endif
|
||||
RMT.chnconf0[channel].idle_out_en_n = 0;
|
||||
RMT.chnconf0[channel].conf_update_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 0;
|
||||
}
|
||||
|
||||
void StepperQueue::startQueue_rmt() {
|
||||
#ifdef TRACE
|
||||
USBSerial.println("START");
|
||||
#endif
|
||||
#ifdef TEST_PROBE_2
|
||||
PROBE_2(LOW);
|
||||
#endif
|
||||
#ifdef TEST_PROBE_3
|
||||
PROBE_3(LOW);
|
||||
#endif
|
||||
#if defined(TEST_PROBE_1) && defined(TEST_PROBE_2) && defined(TEST_PROBE_3)
|
||||
delay(1);
|
||||
PROBE_1_TOGGLE;
|
||||
delay(1);
|
||||
PROBE_1_TOGGLE;
|
||||
delay(1);
|
||||
PROBE_1_TOGGLE;
|
||||
delay(1);
|
||||
PROBE_2_TOGGLE;
|
||||
delay(2);
|
||||
PROBE_2_TOGGLE;
|
||||
delay(2);
|
||||
PROBE_3_TOGGLE;
|
||||
delay(3);
|
||||
PROBE_3_TOGGLE;
|
||||
delay(3);
|
||||
#endif
|
||||
rmt_tx_stop(channel);
|
||||
// rmt_rx_stop(channel);
|
||||
RMT.chnconf0[channel].mem_rd_rst_n = 1;
|
||||
RMT.chnconf0[channel].mem_rd_rst_n = 0;
|
||||
uint32_t *mem = FAS_RMT_MEM(channel);
|
||||
// #define TRACE
|
||||
#ifdef TRACE
|
||||
// Fill the buffer with a significant pattern for debugging
|
||||
// Keep it for now
|
||||
for (uint8_t i = 0; i < 2 * PART_SIZE; i += 2) {
|
||||
// mem[i] = 0x0fff8fff;
|
||||
// mem[i + 1] = 0x7fff8fff;
|
||||
}
|
||||
#endif
|
||||
// Write end marker
|
||||
mem[2 * PART_SIZE] = 0;
|
||||
mem[2 * PART_SIZE + 1] = 0;
|
||||
_isRunning = true;
|
||||
_rmtStopped = false;
|
||||
disable_rmt_interrupts(channel);
|
||||
RMT.chnconf0[channel].mem_tx_wrap_en_n = 0;
|
||||
RMT.chnconf0[channel].conf_update_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 0;
|
||||
|
||||
#ifdef TRACE
|
||||
USBSerial.print("Queue:");
|
||||
USBSerial.print(read_idx);
|
||||
USBSerial.print('/');
|
||||
USBSerial.println(next_write_idx);
|
||||
for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) {
|
||||
USBSerial.print(i);
|
||||
USBSerial.print(' ');
|
||||
USBSerial.println(mem[i], HEX);
|
||||
}
|
||||
#endif
|
||||
|
||||
// set dirpin toggle here
|
||||
uint8_t rp = read_idx;
|
||||
if (rp == next_write_idx) {
|
||||
// nothing to do ?
|
||||
// Should not happen, so bail
|
||||
return;
|
||||
}
|
||||
if (entry[rp & QUEUE_LEN_MASK].toggle_dir) {
|
||||
LL_TOGGLE_PIN(dirPin);
|
||||
entry[rp & QUEUE_LEN_MASK].toggle_dir = false;
|
||||
}
|
||||
|
||||
bufferContainsSteps[0] = true;
|
||||
bufferContainsSteps[1] = true;
|
||||
apply_command(this, true, mem);
|
||||
|
||||
#ifdef TRACE
|
||||
USBSerial.print(RMT.chnconf0[channel].val, BIN);
|
||||
USBSerial.println(' ');
|
||||
USBSerial.print(RMT.chnconf0[channel].mem_tx_wrap_en_n);
|
||||
USBSerial.println(' ');
|
||||
for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) {
|
||||
USBSerial.print(i);
|
||||
USBSerial.print(' ');
|
||||
USBSerial.println(mem[i], HEX);
|
||||
}
|
||||
if (!isRunning()) {
|
||||
USBSerial.println("STOPPED 1");
|
||||
}
|
||||
#endif
|
||||
|
||||
apply_command(this, false, mem);
|
||||
|
||||
#ifdef TRACE
|
||||
USBSerial.print(RMT.chnconf0[channel].val, BIN);
|
||||
USBSerial.print(' ');
|
||||
USBSerial.print(RMT.chnconf0[channel].mem_tx_wrap_en_n);
|
||||
USBSerial.println(' ');
|
||||
|
||||
for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) {
|
||||
USBSerial.print(i);
|
||||
USBSerial.print(' ');
|
||||
USBSerial.println(mem[i], HEX);
|
||||
}
|
||||
#endif
|
||||
enable_rmt_interrupts(channel);
|
||||
|
||||
#ifdef TRACE
|
||||
USBSerial.print("Interrupt enable:");
|
||||
USBSerial.println(RMT.int_ena.val, BIN);
|
||||
USBSerial.print("Interrupt status:");
|
||||
USBSerial.println(RMT.int_st.val, BIN);
|
||||
USBSerial.print("Threshold: 0x");
|
||||
USBSerial.println(RMT.tx_lim[channel].val, HEX);
|
||||
#endif
|
||||
|
||||
// This starts the rmt module
|
||||
RMT.chnconf0[channel].tx_conti_mode_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 0;
|
||||
RMT.chnconf0[channel].mem_tx_wrap_en_n = 0;
|
||||
RMT.chnconf0[channel].conf_update_n = 1;
|
||||
RMT.chnconf0[channel].conf_update_n = 0;
|
||||
|
||||
PROBE_1_TOGGLE;
|
||||
RMT.chnconf0[channel].tx_start_n = 1;
|
||||
}
|
||||
void StepperQueue::forceStop_rmt() {
|
||||
stop_rmt(true);
|
||||
|
||||
// and empty the buffer
|
||||
read_idx = next_write_idx;
|
||||
}
|
||||
bool StepperQueue::isReadyForCommands_rmt() {
|
||||
if (_isRunning) {
|
||||
return !_rmtStopped;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
uint16_t StepperQueue::_getPerformedPulses_rmt() { return 0; }
|
||||
#endif
|
||||
350
src/StepperISR_idf5_esp32_rmt.cpp
Normal file
350
src/StepperISR_idf5_esp32_rmt.cpp
Normal file
@@ -0,0 +1,350 @@
|
||||
#include "StepperISR.h"
|
||||
#if defined(HAVE_ESP32_RMT) && (ESP_IDF_VERSION_MAJOR == 5)
|
||||
|
||||
// #define TEST_MODE
|
||||
|
||||
#include "fas_arch/test_probe.h"
|
||||
|
||||
#define PART_SIZE (RMT_SIZE / 2)
|
||||
|
||||
static bool IRAM_ATTR queue_done(rmt_channel_handle_t tx_chan,
|
||||
const rmt_tx_done_event_data_t *edata,
|
||||
void *user_ctx) {
|
||||
auto *q = (StepperQueue *)user_ctx;
|
||||
q->_isRunning = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
#define ENTER_PAUSE(ticks) \
|
||||
{ \
|
||||
uint16_t remaining_ticks = ticks; \
|
||||
uint16_t half_ticks_per_symbol = (ticks) / (2 * PART_SIZE); \
|
||||
uint32_t main_symbol = 0x00010001 * half_ticks_per_symbol; \
|
||||
for (uint8_t i = 0; i < PART_SIZE - 1; i++) { \
|
||||
(*symbols++).val = main_symbol; \
|
||||
} \
|
||||
remaining_ticks -= 2 * (PART_SIZE - 1) * half_ticks_per_symbol; \
|
||||
uint16_t first_ticks = remaining_ticks / 2; \
|
||||
remaining_ticks -= first_ticks; \
|
||||
last_entry = 0x00010000 * first_ticks + remaining_ticks; \
|
||||
}
|
||||
|
||||
static size_t IRAM_ATTR encode_commands(const void *data, size_t data_size,
|
||||
size_t symbols_written,
|
||||
size_t symbols_free,
|
||||
rmt_symbol_word_t *symbols, bool *done,
|
||||
void *arg) {
|
||||
// this printf causes Guru Meditation
|
||||
// printf("encode commands\n");
|
||||
|
||||
auto *q = (StepperQueue *)arg;
|
||||
|
||||
*done = false;
|
||||
if (symbols_free < PART_SIZE) {
|
||||
// not sufficient space for the symbols
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint8_t rp = q->read_idx;
|
||||
if (q->_rmtStopped) {
|
||||
*done = true;
|
||||
return 0;
|
||||
}
|
||||
if ((rp == q->next_write_idx) || q->_rmtStopped) {
|
||||
// if we return done already here, then single stepping fails
|
||||
q->_rmtStopped = true;
|
||||
// Not sure if this pause is really needed
|
||||
uint16_t last_entry;
|
||||
ENTER_PAUSE(MIN_CMD_TICKS);
|
||||
symbols->val = last_entry;
|
||||
return PART_SIZE;
|
||||
}
|
||||
|
||||
// Process command
|
||||
struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK];
|
||||
|
||||
if (e_curr->toggle_dir) {
|
||||
// the command requests dir pin toggle
|
||||
// This is ok only, if the ongoing command does not contain steps
|
||||
if (q->lastChunkContainsSteps) {
|
||||
// So we need a pause. change the finished read entry into a pause
|
||||
q->lastChunkContainsSteps = false;
|
||||
uint16_t last_entry;
|
||||
ENTER_PAUSE(MIN_CMD_TICKS);
|
||||
symbols->val = last_entry;
|
||||
return PART_SIZE;
|
||||
}
|
||||
// The ongoing command does not contain steps, so change dir here should be
|
||||
// ok
|
||||
LL_TOGGLE_PIN(q->dirPin);
|
||||
// and delete the request
|
||||
e_curr->toggle_dir = 0;
|
||||
}
|
||||
|
||||
uint8_t steps = e_curr->steps;
|
||||
uint16_t ticks = e_curr->ticks;
|
||||
// if (steps != 0) {
|
||||
// PROBE_2_TOGGLE;
|
||||
//}
|
||||
uint32_t last_entry;
|
||||
if (steps == 0) {
|
||||
q->lastChunkContainsSteps = false;
|
||||
ENTER_PAUSE(ticks);
|
||||
} else {
|
||||
q->lastChunkContainsSteps = true;
|
||||
if (ticks == 0xffff) {
|
||||
// special treatment for this case, because an rmt entry can only cover up
|
||||
// to 0xfffe ticks every step must be minimum split into two rmt entries,
|
||||
// so at max PART/2 steps can be done.
|
||||
if (steps < PART_SIZE / 2) {
|
||||
for (uint8_t i = 1; i < steps; i++) {
|
||||
// steps-1 iterations
|
||||
(*symbols++).val = 0x40007fff | 0x8000;
|
||||
(*symbols++).val = 0x20002000;
|
||||
}
|
||||
// the last step needs to be stretched to fill PART_SIZE entries
|
||||
(*symbols++).val = 0x40007fff | 0x8000;
|
||||
uint16_t delta = PART_SIZE - 2 * steps;
|
||||
delta <<= 5;
|
||||
(*symbols++).val = 0x20002000 - delta;
|
||||
// 2*(steps - 1) + 1 already stored => 2*steps - 1
|
||||
// and after this for loop one entry added => 2*steps
|
||||
for (uint8_t i = 2 * steps; i < PART_SIZE - 1; i++) {
|
||||
(*symbols++).val = 0x00100010;
|
||||
}
|
||||
last_entry = 0x00100010;
|
||||
steps = 0;
|
||||
} else {
|
||||
steps -= PART_SIZE / 2;
|
||||
for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) {
|
||||
(*symbols++).val = 0x40007fff | 0x8000;
|
||||
(*symbols++).val = 0x20002000;
|
||||
}
|
||||
(*symbols++).val = 0x40007fff | 0x8000;
|
||||
last_entry = 0x20002000;
|
||||
}
|
||||
} else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) {
|
||||
uint8_t steps_to_do = steps;
|
||||
if (steps > PART_SIZE) {
|
||||
steps_to_do /= 2;
|
||||
}
|
||||
|
||||
uint16_t ticks_l = ticks >> 1;
|
||||
uint16_t ticks_r = ticks - ticks_l;
|
||||
uint32_t rmt_entry = ticks_l;
|
||||
rmt_entry <<= 16;
|
||||
rmt_entry |= ticks_r | 0x8000; // with step
|
||||
for (uint8_t i = 1; i < steps_to_do; i++) {
|
||||
(*symbols++).val = rmt_entry;
|
||||
}
|
||||
// the last step needs to be stretched to fill PART_SIZE entries
|
||||
uint32_t delta = PART_SIZE - steps_to_do;
|
||||
delta <<= 18; // shift in upper 16bit and multiply with 4
|
||||
(*symbols++).val = rmt_entry - delta;
|
||||
for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) {
|
||||
(*symbols++).val = 0x00020002;
|
||||
}
|
||||
last_entry = 0x00020002;
|
||||
steps -= steps_to_do;
|
||||
} else {
|
||||
// either >= 2*PART_SIZE or = PART_SIZE
|
||||
// every entry one step
|
||||
uint16_t ticks_l = ticks >> 1;
|
||||
uint16_t ticks_r = ticks - ticks_l;
|
||||
uint32_t rmt_entry = ticks_l;
|
||||
rmt_entry <<= 16;
|
||||
rmt_entry |= ticks_r | 0x8000; // with step
|
||||
for (uint8_t i = 0; i < PART_SIZE - 1; i++) {
|
||||
(*symbols++).val = rmt_entry;
|
||||
}
|
||||
last_entry = rmt_entry;
|
||||
steps -= PART_SIZE;
|
||||
}
|
||||
}
|
||||
|
||||
// if (!fill_part_one) {
|
||||
// Note: When enabling the continuous transmission mode by setting
|
||||
// RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the
|
||||
// channel continuously, that is, from the first byte to the last one,
|
||||
// then from the first to the last again, and so on. In this mode, there
|
||||
// will be an idle level lasting one clk_div cycle between N and N+1
|
||||
// transmissions.
|
||||
// last_entry -= 1;
|
||||
//}
|
||||
symbols->val = last_entry;
|
||||
|
||||
// Data is complete
|
||||
if (steps == 0) {
|
||||
// The command has been completed
|
||||
if (e_curr->repeat_entry == 0) {
|
||||
q->read_idx = rp + 1;
|
||||
}
|
||||
} else {
|
||||
e_curr->steps = steps;
|
||||
}
|
||||
|
||||
return PART_SIZE;
|
||||
}
|
||||
|
||||
void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {
|
||||
#ifdef TEST_PROBE_1
|
||||
if (channel_num == 0) {
|
||||
pinMode(TEST_PROBE_1, OUTPUT);
|
||||
PROBE_1(HIGH);
|
||||
delay(10);
|
||||
PROBE_1(LOW);
|
||||
delay(5);
|
||||
PROBE_1(HIGH);
|
||||
delay(5);
|
||||
PROBE_1(LOW);
|
||||
delay(5);
|
||||
}
|
||||
#endif
|
||||
#ifdef TEST_PROBE_2
|
||||
pinMode(TEST_PROBE_2, OUTPUT);
|
||||
PROBE_2(LOW);
|
||||
#endif
|
||||
#ifdef TEST_PROBE_3
|
||||
pinMode(TEST_PROBE_3, OUTPUT);
|
||||
PROBE_3(LOW);
|
||||
#endif
|
||||
|
||||
_initVars();
|
||||
_step_pin = step_pin;
|
||||
pinMode(step_pin, OUTPUT);
|
||||
digitalWrite(step_pin, LOW);
|
||||
|
||||
rmt_simple_encoder_config_t enc_config = {
|
||||
.callback = encode_commands, .arg = this, .min_chunk_size = PART_SIZE};
|
||||
esp_err_t rc;
|
||||
rc = rmt_new_simple_encoder(&enc_config, &_tx_encoder);
|
||||
ESP_ERROR_CHECK_WITHOUT_ABORT(rc);
|
||||
|
||||
connect_rmt();
|
||||
_isRunning = false;
|
||||
_rmtStopped = true;
|
||||
}
|
||||
|
||||
void StepperQueue::connect_rmt() {
|
||||
rmt_tx_channel_config_t config;
|
||||
config.gpio_num = (gpio_num_t)_step_pin;
|
||||
config.clk_src = RMT_CLK_SRC_DEFAULT;
|
||||
config.resolution_hz = TICKS_PER_S;
|
||||
config.mem_block_symbols = 2 * PART_SIZE;
|
||||
config.trans_queue_depth = 1;
|
||||
config.intr_priority = 0;
|
||||
config.flags.invert_out = 0;
|
||||
config.flags.with_dma = 0;
|
||||
config.flags.io_loop_back = 0;
|
||||
config.flags.io_od_mode = 0;
|
||||
esp_err_t rc = rmt_new_tx_channel(&config, &channel);
|
||||
ESP_ERROR_CHECK_WITHOUT_ABORT(rc);
|
||||
|
||||
rmt_tx_event_callbacks_t callbacks = {.on_trans_done = queue_done};
|
||||
(void)rmt_tx_register_event_callbacks(channel, &callbacks, this);
|
||||
|
||||
_channel_enabled = false;
|
||||
}
|
||||
|
||||
void StepperQueue::disconnect_rmt() {
|
||||
if (_channel_enabled || _isRunning || !_rmtStopped) {
|
||||
return;
|
||||
}
|
||||
(void)rmt_del_channel(channel);
|
||||
channel = nullptr;
|
||||
}
|
||||
|
||||
void StepperQueue::startQueue_rmt() {
|
||||
// #define TRACE
|
||||
#ifdef TRACE
|
||||
Serial.println("START");
|
||||
#endif
|
||||
#ifdef TEST_PROBE_2
|
||||
PROBE_2(LOW);
|
||||
#endif
|
||||
#ifdef TEST_PROBE_3
|
||||
PROBE_3(LOW);
|
||||
#endif
|
||||
#ifdef TEST_PROBE_1
|
||||
delay(1);
|
||||
PROBE_1_TOGGLE;
|
||||
delay(1);
|
||||
PROBE_1_TOGGLE;
|
||||
delay(1);
|
||||
PROBE_1_TOGGLE;
|
||||
delay(1);
|
||||
#endif
|
||||
|
||||
// #define TRACE
|
||||
#ifdef TRACE
|
||||
printf("Queue: %d/%d %s\n", read_idx, next_write_idx,
|
||||
_isRunning ? "Running" : "Stopped");
|
||||
#endif
|
||||
|
||||
if (channel == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
// set dirpin toggle here
|
||||
uint8_t rp = read_idx;
|
||||
if (rp == next_write_idx) {
|
||||
// nothing to do ?
|
||||
// Should not happen, so bail
|
||||
return;
|
||||
}
|
||||
if (entry[rp & QUEUE_LEN_MASK].toggle_dir) {
|
||||
LL_TOGGLE_PIN(dirPin);
|
||||
entry[rp & QUEUE_LEN_MASK].toggle_dir = false;
|
||||
}
|
||||
|
||||
#ifdef TRACE
|
||||
queue_entry *e = &entry[rp & QUEUE_LEN_MASK];
|
||||
printf("first command: ticks=%u steps=%u %s %s\n", e->ticks, e->steps,
|
||||
e->countUp ? "up" : "down", e->toggle_dir ? "toggle" : "");
|
||||
#endif
|
||||
|
||||
if (_channel_enabled) {
|
||||
// rmt_disable(channel);
|
||||
// _channel_enabled = false;
|
||||
}
|
||||
|
||||
lastChunkContainsSteps = false;
|
||||
_isRunning = true;
|
||||
_rmtStopped = false;
|
||||
|
||||
// payload and payload bytes must not be 0
|
||||
int payload = 0;
|
||||
rmt_transmit_config_t tx_config;
|
||||
tx_config.loop_count = 0;
|
||||
tx_config.flags.eot_level = 0; // output level at end of transmission
|
||||
tx_config.flags.queue_nonblocking = 1;
|
||||
_tx_encoder->reset(_tx_encoder);
|
||||
|
||||
if (!_channel_enabled) {
|
||||
rmt_enable(channel);
|
||||
_channel_enabled = true;
|
||||
}
|
||||
|
||||
esp_err_t rc = rmt_transmit(channel, _tx_encoder, &payload, 1, &tx_config);
|
||||
ESP_ERROR_CHECK_WITHOUT_ABORT(rc);
|
||||
#ifdef TRACE
|
||||
printf("Transmission started\n");
|
||||
#endif
|
||||
}
|
||||
void StepperQueue::forceStop_rmt() {
|
||||
(void)rmt_disable(channel);
|
||||
_channel_enabled = false;
|
||||
_isRunning = false;
|
||||
_rmtStopped = true;
|
||||
|
||||
// and empty the buffer
|
||||
read_idx = next_write_idx;
|
||||
}
|
||||
bool StepperQueue::isReadyForCommands_rmt() const {
|
||||
if (_isRunning) {
|
||||
return !_rmtStopped;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
uint16_t StepperQueue::_getPerformedPulses_rmt() { return 0; }
|
||||
#endif
|
||||
83
src/fas_arch/arduino_avr.h
Normal file
83
src/fas_arch/arduino_avr.h
Normal file
@@ -0,0 +1,83 @@
|
||||
#ifndef FAS_ARCH_ARDUINO_AVR_H
|
||||
#define FAS_ARCH_ARDUINO_AVR_H
|
||||
|
||||
#define SUPPORT_AVR
|
||||
#define SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING 1
|
||||
|
||||
// this is an arduino platform, so include the Arduino.h header file
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "AVRStepperPins.h"
|
||||
// for AVR processors a reentrant version of disabling/enabling interrupts is
|
||||
// used
|
||||
#define fasDisableInterrupts() \
|
||||
uint8_t prevSREG = SREG; \
|
||||
cli()
|
||||
#define fasEnableInterrupts() SREG = prevSREG
|
||||
|
||||
// Here are shorthand definitions for number of queues, the queues/channel
|
||||
// relation and queue length This definitions are derivate specific
|
||||
#define QUEUE_LEN 16
|
||||
#define TICKS_PER_S F_CPU
|
||||
#define MIN_CMD_TICKS (TICKS_PER_S / 25000)
|
||||
#define MIN_DIR_DELAY_US 40
|
||||
#define MAX_DIR_DELAY_US (65535 / (TICKS_PER_S / 1000000))
|
||||
#define DELAY_MS_BASE (65536000 / TICKS_PER_S)
|
||||
|
||||
// debug led timing
|
||||
#define DEBUG_LED_HALF_PERIOD (TICKS_PER_S / 65536 / 2)
|
||||
|
||||
#define noop_or_wait
|
||||
|
||||
#define SUPPORT_DIR_TOGGLE_PIN_MASK uint8_t
|
||||
|
||||
#define SUPPORT_QUEUE_ENTRY_END_POS_U16
|
||||
|
||||
//==========================================================================
|
||||
//
|
||||
// AVR derivate ATmega 168/328/P
|
||||
//
|
||||
//==========================================================================
|
||||
#if (defined(__AVR_ATmega168__) || defined(__AVR_ATmega168P__) || \
|
||||
defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__))
|
||||
#define SUPPORT_EXTERNAL_DIRECTION_PIN
|
||||
#define MAX_STEPPER 2
|
||||
#define NUM_QUEUES 2
|
||||
#define fas_queue_A fas_queue[0]
|
||||
#define fas_queue_B fas_queue[1]
|
||||
enum channels { channelA, channelB };
|
||||
//==========================================================================
|
||||
//
|
||||
// AVR derivate ATmega 2560
|
||||
//
|
||||
//==========================================================================
|
||||
#elif defined(__AVR_ATmega2560__)
|
||||
#define SUPPORT_EXTERNAL_DIRECTION_PIN
|
||||
#define MAX_STEPPER 3
|
||||
#define NUM_QUEUES 3
|
||||
#define fas_queue_A fas_queue[0]
|
||||
#define fas_queue_B fas_queue[1]
|
||||
#define fas_queue_C fas_queue[2]
|
||||
enum channels { channelA, channelB, channelC };
|
||||
//==========================================================================
|
||||
//
|
||||
// AVR derivate ATmega 32U4
|
||||
//
|
||||
//==========================================================================
|
||||
#elif defined(__AVR_ATmega32U4__)
|
||||
#define MAX_STEPPER 3
|
||||
#define NUM_QUEUES 3
|
||||
#define fas_queue_A fas_queue[0]
|
||||
#define fas_queue_B fas_queue[1]
|
||||
#define fas_queue_C fas_queue[2]
|
||||
enum channels { channelA, channelB, channelC };
|
||||
//==========================================================================
|
||||
//
|
||||
// For all unsupported AVR derivates
|
||||
//
|
||||
//==========================================================================
|
||||
#else
|
||||
#error "Unsupported AVR derivate"
|
||||
#endif
|
||||
|
||||
#endif /* FAS_ARCH_ARDUINO_AVR_H */
|
||||
13
src/fas_arch/arduino_esp32.h
Normal file
13
src/fas_arch/arduino_esp32.h
Normal file
@@ -0,0 +1,13 @@
|
||||
#ifndef FAS_ARCH_ARDUINO_ESP32_H
|
||||
#define FAS_ARCH_ARDUINO_ESP32_H
|
||||
|
||||
// this is an arduino platform, so include the Arduino.h header file
|
||||
#include <Arduino.h>
|
||||
|
||||
// For esp32 using arduino, just use arduino definition
|
||||
#define fasEnableInterrupts interrupts
|
||||
#define fasDisableInterrupts noInterrupts
|
||||
|
||||
#include "fas_arch/common_esp32.h"
|
||||
|
||||
#endif /* FAS_ARCH_ARDUINO_ESP32_H */
|
||||
34
src/fas_arch/arduino_sam.h
Normal file
34
src/fas_arch/arduino_sam.h
Normal file
@@ -0,0 +1,34 @@
|
||||
#ifndef FAS_ARCH_ARDUINO_SAM_H
|
||||
#define FAS_ARCH_ARDUINO_SAM_H
|
||||
|
||||
#define SUPPORT_SAM
|
||||
|
||||
// this is an arduino platform, so include the Arduino.h header file
|
||||
#include <Arduino.h>
|
||||
// on SAM just use the arduino macros
|
||||
#define fasEnableInterrupts interrupts
|
||||
#define fasDisableInterrupts noInterrupts
|
||||
|
||||
// queue definitions for SAM
|
||||
#define MAX_STEPPER 6
|
||||
#define NUM_QUEUES 6
|
||||
#define QUEUE_LEN 32
|
||||
|
||||
// timing definitions for SAM
|
||||
#define TICKS_PER_S 21000000L
|
||||
#define MIN_CMD_TICKS (TICKS_PER_S / 5000)
|
||||
#define MIN_DIR_DELAY_US (MIN_CMD_TICKS / (TICKS_PER_S / 1000000))
|
||||
#define MAX_DIR_DELAY_US (65535 / (TICKS_PER_S / 1000000))
|
||||
#define DELAY_MS_BASE 2
|
||||
|
||||
// debug led timing
|
||||
#define DEBUG_LED_HALF_PERIOD 50
|
||||
|
||||
#define noop_or_wait
|
||||
|
||||
#define SUPPORT_DIR_PIN_MASK uint32_t
|
||||
|
||||
// TO BE CHECKED
|
||||
#define SUPPORT_QUEUE_ENTRY_END_POS_U16
|
||||
|
||||
#endif /* FAS_ARCH_ARDUINO_SAM_H */
|
||||
105
src/fas_arch/common.h
Normal file
105
src/fas_arch/common.h
Normal file
@@ -0,0 +1,105 @@
|
||||
#ifndef FAS_COMMON_H
|
||||
#define FAS_COMMON_H
|
||||
|
||||
#define TICKS_FOR_STOPPED_MOTOR 0xffffffff
|
||||
|
||||
#define MOVE_OK 0
|
||||
#define MOVE_ERR_NO_DIRECTION_PIN -1
|
||||
#define MOVE_ERR_SPEED_IS_UNDEFINED -2
|
||||
#define MOVE_ERR_ACCELERATION_IS_UNDEFINED -3
|
||||
|
||||
// Low level stepper motor command.
|
||||
//
|
||||
// You can add these using the addQueueEntry method.
|
||||
// They will be executed sequentially until the queue is empty.
|
||||
//
|
||||
// There are some constraints on the values:
|
||||
// - `ticks` must be greater or equal to FastAccelStepper::getMaxSpeedInTicks.
|
||||
// - `ticks*steps` must be greater or equal to MIN_CMD_TICKS
|
||||
//
|
||||
// For example:
|
||||
// A command with ticks=TICKS_PER_S/1000, steps = 3, count_up = true means that:
|
||||
// 1. The direction pin is set to HIGH.
|
||||
// 2. One step is generated.
|
||||
// 3. Exactly 1 ms after the first step, the second step is issued.
|
||||
// 4. Exactly 1 ms after the second step, the third step is issued.
|
||||
// 5. The stepper waits for 1 ms.
|
||||
// 6. The next command is processed.
|
||||
struct stepper_command_s {
|
||||
// Number of ticks between each step.
|
||||
// There are `TICKS_PER_S` ticks per second. This may vary between different
|
||||
// platforms.
|
||||
uint16_t ticks;
|
||||
|
||||
// Number of steps to send to the stepper motor during this command.
|
||||
// If zero, then this command will be treated as a pause, lasting for a number
|
||||
// of ticks given by `ticks`.
|
||||
uint8_t steps;
|
||||
|
||||
// True if the direction pin should be high during this command, false if it
|
||||
// should be low.
|
||||
bool count_up;
|
||||
};
|
||||
|
||||
struct actual_ticks_s {
|
||||
uint32_t ticks; // ticks == 0 means standstill
|
||||
bool count_up;
|
||||
};
|
||||
|
||||
// I doubt, volatile is needed.
|
||||
struct queue_end_s {
|
||||
volatile int32_t pos; // in steps
|
||||
volatile bool count_up;
|
||||
volatile bool dir;
|
||||
};
|
||||
|
||||
// use own min/max/abs function, because the lib versions are messed up
|
||||
#define fas_min(a, b) ((a) > (b) ? (b) : (a))
|
||||
#define fas_max(a, b) ((a) > (b) ? (a) : (b))
|
||||
#define fas_abs(x) ((x) >= 0 ? (x) : (-x))
|
||||
|
||||
//==============================================================================
|
||||
// All architecture specific definitions should be located here
|
||||
//==============================================================================
|
||||
|
||||
//==========================================================================
|
||||
#if defined(TEST)
|
||||
// TEST "architecture" is in use with pc_based testing.
|
||||
#include "fas_arch/test_pc.h"
|
||||
|
||||
#elif defined(ARDUINO_ARCH_ESP32)
|
||||
// ESP32 derivates using arduino core
|
||||
#include "fas_arch/arduino_esp32.h"
|
||||
|
||||
#elif defined(ESP_PLATFORM)
|
||||
// ESP32 derivates using espidf
|
||||
#include "fas_arch/espidf_esp32.h"
|
||||
|
||||
#elif defined(ARDUINO_ARCH_SAM)
|
||||
// SAM-architecture
|
||||
#include "fas_arch/arduino_sam.h"
|
||||
|
||||
#elif defined(ARDUINO_ARCH_AVR)
|
||||
// AVR family
|
||||
#include "fas_arch/arduino_avr.h"
|
||||
|
||||
#else
|
||||
#error "Unsupported devices"
|
||||
#endif
|
||||
|
||||
// in order to avoid spikes, first set the value and then make an output
|
||||
// esp32 idf5 does not like this approach
|
||||
#ifndef PIN_OUTPUT
|
||||
#define PIN_OUTPUT(pin, value) \
|
||||
{ \
|
||||
digitalWrite(pin, (value)); \
|
||||
pinMode(pin, OUTPUT); \
|
||||
}
|
||||
#endif
|
||||
|
||||
// disable inject_fill_interrupt() for all real devices. Only defined in TEST
|
||||
#ifndef TEST
|
||||
#define inject_fill_interrupt(x)
|
||||
#endif
|
||||
|
||||
#endif /* FAS_COMMON_H */
|
||||
60
src/fas_arch/common_esp32.h
Normal file
60
src/fas_arch/common_esp32.h
Normal file
@@ -0,0 +1,60 @@
|
||||
#ifndef FAS_ARCH_COMMON_ESP32_H
|
||||
#define FAS_ARCH_COMMON_ESP32_H
|
||||
|
||||
#include <sdkconfig.h>
|
||||
|
||||
#define SUPPORT_ESP32
|
||||
#define SUPPORT_EXTERNAL_DIRECTION_PIN
|
||||
#define SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING 1
|
||||
|
||||
// Some more esp32 specific includes
|
||||
#include <hal/gpio_ll.h>
|
||||
#include <driver/gpio.h>
|
||||
#include <esp_task_wdt.h>
|
||||
|
||||
#if ESP_IDF_VERSION_MAJOR == 5
|
||||
#if ESP_IDF_VERSION_MINOR < 3
|
||||
#error "FastAccelStepper requires esp-idf >= 5.3.0"
|
||||
#endif
|
||||
#include "fas_arch/common_esp32_idf5.h"
|
||||
#elif ESP_IDF_VERSION_MAJOR == 4
|
||||
#include "fas_arch/common_esp32_idf4.h"
|
||||
#elif ESP_IDF_VERSION_MAJOR <= 3
|
||||
#pragma "Last supported by FastAccelStepper 0.30.15"
|
||||
#endif
|
||||
|
||||
// Esp32 queue definitions
|
||||
#define NUM_QUEUES (QUEUES_MCPWM_PCNT + QUEUES_RMT)
|
||||
#define MAX_STEPPER (NUM_QUEUES)
|
||||
#define QUEUE_LEN 32
|
||||
|
||||
// Esp32 timing definition
|
||||
#define TICKS_PER_S 16000000L
|
||||
#define MIN_CMD_TICKS (TICKS_PER_S / 5000)
|
||||
#define MIN_DIR_DELAY_US (MIN_CMD_TICKS / (TICKS_PER_S / 1000000))
|
||||
#define MAX_DIR_DELAY_US (65535 / (TICKS_PER_S / 1000000))
|
||||
#define DELAY_MS_BASE 4
|
||||
|
||||
#define SUPPORT_QUEUE_ENTRY_START_POS_U16
|
||||
|
||||
// debug led timing
|
||||
#define DEBUG_LED_HALF_PERIOD 50
|
||||
|
||||
#define noop_or_wait vTaskDelay(1)
|
||||
|
||||
// have more than one core
|
||||
#define SUPPORT_CPU_AFFINITY
|
||||
|
||||
#define LL_TOGGLE_PIN(dirPin) \
|
||||
gpio_ll_set_level(&GPIO, (gpio_num_t)dirPin, \
|
||||
gpio_ll_get_level(&GPIO, (gpio_num_t)dirPin) ^ 1)
|
||||
|
||||
//==========================================================================
|
||||
// determine, if driver type selection should be supported
|
||||
#if defined(QUEUES_MCPWM_PCNT) && defined(QUEUES_RMT)
|
||||
#if (QUEUES_MCPWM_PCNT > 0) && (QUEUES_RMT > 0)
|
||||
#define SUPPORT_SELECT_DRIVER_TYPE
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#endif /* FAS_ARCH_COMMON_ESP32_H */
|
||||
104
src/fas_arch/common_esp32_idf4.h
Normal file
104
src/fas_arch/common_esp32_idf4.h
Normal file
@@ -0,0 +1,104 @@
|
||||
#ifndef FAS_ARCH_COMMON_ESP32_IDF4_H
|
||||
#define FAS_ARCH_COMMON_ESP32_IDF4_H
|
||||
|
||||
//==========================================================================
|
||||
//
|
||||
// ESP32 derivate - the first one
|
||||
//
|
||||
//==========================================================================
|
||||
|
||||
#if CONFIG_IDF_TARGET_ESP32
|
||||
#define SUPPORT_ESP32_MCPWM_PCNT
|
||||
#define SUPPORT_ESP32_RMT
|
||||
#define SUPPORT_ESP32_PULSE_COUNTER 8
|
||||
#define HAVE_ESP32_RMT
|
||||
#define QUEUES_MCPWM_PCNT 6
|
||||
#define QUEUES_RMT 8
|
||||
#define NEED_RMT_HEADERS
|
||||
#define NEED_MCPWM_HEADERS
|
||||
#define NEED_PCNT_HEADERS
|
||||
|
||||
//==========================================================================
|
||||
//
|
||||
// ESP32 derivate - ESP32S2
|
||||
//
|
||||
//==========================================================================
|
||||
#elif CONFIG_IDF_TARGET_ESP32S2
|
||||
#define SUPPORT_ESP32_RMT
|
||||
#define SUPPORT_ESP32_PULSE_COUNTER 4
|
||||
#define HAVE_ESP32S3_PULSE_COUNTER
|
||||
#define HAVE_ESP32_RMT
|
||||
#define QUEUES_MCPWM_PCNT 0
|
||||
#define QUEUES_RMT 4
|
||||
#define NEED_RMT_HEADERS
|
||||
#define NEED_PCNT_HEADERS
|
||||
|
||||
//==========================================================================
|
||||
//
|
||||
// ESP32 derivate - ESP32S3
|
||||
//
|
||||
//==========================================================================
|
||||
#elif CONFIG_IDF_TARGET_ESP32S3
|
||||
#define SUPPORT_ESP32_MCPWM_PCNT
|
||||
#define SUPPORT_ESP32_RMT
|
||||
#define SUPPORT_ESP32_PULSE_COUNTER 4
|
||||
#define HAVE_ESP32S3_PULSE_COUNTER
|
||||
#define HAVE_ESP32S3_RMT
|
||||
|
||||
#define QUEUES_MCPWM_PCNT 4
|
||||
#define QUEUES_RMT 4
|
||||
#define NEED_RMT_HEADERS
|
||||
#define NEED_MCPWM_HEADERS
|
||||
#define NEED_PCNT_HEADERS
|
||||
|
||||
//==========================================================================
|
||||
//
|
||||
// ESP32 derivate - ESP32C3
|
||||
//
|
||||
//==========================================================================
|
||||
#elif CONFIG_IDF_TARGET_ESP32C3
|
||||
#define SUPPORT_ESP32_RMT
|
||||
#define HAVE_ESP32C3_RMT
|
||||
#define QUEUES_MCPWM_PCNT 0
|
||||
#define QUEUES_RMT 2
|
||||
#define NEED_RMT_HEADERS
|
||||
|
||||
//==========================================================================
|
||||
//
|
||||
// For all unsupported ESP32 derivates
|
||||
//
|
||||
//==========================================================================
|
||||
#else
|
||||
#error "Unsupported derivate"
|
||||
#endif
|
||||
|
||||
#if ESP_IDF_VERSION_MINOR == 4
|
||||
#define __ESP32_IDF_V44__
|
||||
#endif
|
||||
|
||||
#include <driver/periph_ctrl.h>
|
||||
#include <soc/periph_defs.h>
|
||||
|
||||
#ifdef NEED_MCPWM_HEADERS
|
||||
#include <driver/mcpwm.h>
|
||||
#include <soc/mcpwm_reg.h>
|
||||
#include <soc/mcpwm_struct.h>
|
||||
#endif
|
||||
|
||||
#ifdef NEED_PCNT_HEADERS
|
||||
#include <driver/pcnt.h>
|
||||
#include <soc/pcnt_reg.h>
|
||||
#include <soc/pcnt_struct.h>
|
||||
#endif
|
||||
|
||||
#ifdef NEED_RMT_HEADERS
|
||||
#include <driver/rmt.h>
|
||||
#include <soc/rmt_periph.h>
|
||||
#include <soc/rmt_reg.h>
|
||||
#include <soc/rmt_struct.h>
|
||||
|
||||
#define RMT_CHANNEL_T rmt_channel_t
|
||||
#define FAS_RMT_MEM(channel) ((uint32_t *)RMTMEM.chan[channel].data32)
|
||||
#endif
|
||||
|
||||
#endif /* FAS_ARCH_COMMON_ESP32_IDF4_H */
|
||||
152
src/fas_arch/common_esp32_idf5.h
Normal file
152
src/fas_arch/common_esp32_idf5.h
Normal file
@@ -0,0 +1,152 @@
|
||||
#ifndef FAS_ARCH_COMMON_ESP32_IDF5_H
|
||||
#define FAS_ARCH_COMMON_ESP32_IDF5_H
|
||||
|
||||
//==========================================================================
|
||||
//
|
||||
// ESP32 derivate - the first one
|
||||
//
|
||||
//==========================================================================
|
||||
|
||||
#if CONFIG_IDF_TARGET_ESP32
|
||||
// #define SUPPORT_ESP32_MCPWM_PCNT
|
||||
#define SUPPORT_ESP32_RMT
|
||||
#define SUPPORT_ESP32_PULSE_COUNTER 8
|
||||
#define HAVE_ESP32_RMT
|
||||
#define RMT_SIZE 64
|
||||
|
||||
// #define QUEUES_MCPWM_PCNT 6
|
||||
#define QUEUES_MCPWM_PCNT 0
|
||||
#define QUEUES_RMT 8
|
||||
|
||||
#define NEED_RMT_HEADERS
|
||||
// #define NEED_MCPWM_HEADERS
|
||||
#define NEED_PCNT_HEADERS
|
||||
|
||||
//==========================================================================
|
||||
//
|
||||
// ESP32 derivate - ESP32S2
|
||||
//
|
||||
//==========================================================================
|
||||
#elif CONFIG_IDF_TARGET_ESP32S2
|
||||
#define SUPPORT_ESP32_RMT
|
||||
#define SUPPORT_ESP32_PULSE_COUNTER 4
|
||||
#define HAVE_ESP32S3_PULSE_COUNTER
|
||||
#define HAVE_ESP32_RMT
|
||||
#define RMT_SIZE 64
|
||||
#define QUEUES_MCPWM_PCNT 0
|
||||
#define QUEUES_RMT 4
|
||||
#define NEED_RMT_HEADERS
|
||||
#define NEED_PCNT_HEADERS
|
||||
|
||||
//==========================================================================
|
||||
//
|
||||
// ESP32 derivate - ESP32S3
|
||||
//
|
||||
//==========================================================================
|
||||
#elif CONFIG_IDF_TARGET_ESP32S3
|
||||
// #define SUPPORT_ESP32_MCPWM_PCNT
|
||||
#define SUPPORT_ESP32_RMT
|
||||
#define SUPPORT_ESP32_PULSE_COUNTER 8
|
||||
#define HAVE_ESP32S3_PULSE_COUNTER
|
||||
#define HAVE_ESP32_RMT
|
||||
#define RMT_SIZE 48
|
||||
|
||||
// #define QUEUES_MCPWM_PCNT 4
|
||||
#define QUEUES_MCPWM_PCNT 0
|
||||
#define QUEUES_RMT 4
|
||||
#define NEED_RMT_HEADERS
|
||||
// #define NEED_MCPWM_HEADERS
|
||||
#define NEED_PCNT_HEADERS
|
||||
|
||||
//==========================================================================
|
||||
//
|
||||
// ESP32 derivate - ESP32C3
|
||||
//
|
||||
//==========================================================================
|
||||
#elif CONFIG_IDF_TARGET_ESP32C3
|
||||
#define SUPPORT_ESP32_RMT
|
||||
#define HAVE_ESP32_RMT
|
||||
#define RMT_SIZE 48
|
||||
#define QUEUES_MCPWM_PCNT 0
|
||||
#define QUEUES_RMT 2
|
||||
#define NEED_RMT_HEADERS
|
||||
|
||||
//==========================================================================
|
||||
//
|
||||
// ESP32 derivate - ESP32C6
|
||||
//
|
||||
//==========================================================================
|
||||
#elif CONFIG_IDF_TARGET_ESP32C6
|
||||
#define SUPPORT_ESP32_RMT
|
||||
#define SUPPORT_ESP32_PULSE_COUNTER 4
|
||||
#define HAVE_ESP32_RMT
|
||||
#define RMT_SIZE 48
|
||||
#define QUEUES_MCPWM_PCNT 0
|
||||
#define QUEUES_RMT 2
|
||||
#define NEED_RMT_HEADERS
|
||||
#define NEED_PCNT_HEADERS
|
||||
|
||||
//==========================================================================
|
||||
//
|
||||
// ESP32 derivate - ESP32H2
|
||||
//
|
||||
//==========================================================================
|
||||
#elif CONFIG_IDF_TARGET_ESP32H2
|
||||
#define SUPPORT_ESP32_RMT
|
||||
#define SUPPORT_ESP32_PULSE_COUNTER 4
|
||||
#define HAVE_ESP32_RMT
|
||||
#define RMT_SIZE 48
|
||||
#define QUEUES_MCPWM_PCNT 0
|
||||
#define QUEUES_RMT 2
|
||||
#define NEED_RMT_HEADERS
|
||||
#define NEED_PCNT_HEADERS
|
||||
|
||||
//==========================================================================
|
||||
//
|
||||
// For all unsupported ESP32 derivates
|
||||
//
|
||||
//==========================================================================
|
||||
#else
|
||||
#error "Unsupported derivate"
|
||||
#endif
|
||||
|
||||
// #include <driver/periph_ctrl.h>
|
||||
#include <soc/periph_defs.h>
|
||||
#include <soc/gpio_sig_map.h>
|
||||
|
||||
#ifdef NEED_MCPWM_HEADERS
|
||||
#include <driver/mcpwm.h>
|
||||
#include <soc/mcpwm_reg.h>
|
||||
#include <soc/mcpwm_struct.h>
|
||||
#endif
|
||||
|
||||
#ifdef NEED_PCNT_HEADERS
|
||||
#include <driver/pulse_cnt.h>
|
||||
// #include <soc/pcnt_reg.h>
|
||||
// #include <soc/pcnt_struct.h>
|
||||
#include <soc/pcnt_periph.h>
|
||||
#include <driver/gpio.h>
|
||||
#include <rom/gpio.h>
|
||||
#include <hal/gpio_ll.h>
|
||||
#include <esp_rom_gpio.h>
|
||||
#endif
|
||||
|
||||
#ifdef NEED_RMT_HEADERS
|
||||
#include <driver/rmt_tx.h>
|
||||
#include <soc/rmt_periph.h>
|
||||
#include <soc/rmt_reg.h>
|
||||
#include <soc/rmt_struct.h>
|
||||
|
||||
#define RMT_CHANNEL_T rmt_channel_handle_t
|
||||
#define FAS_RMT_MEM(channel) ((uint32_t *)RMTMEM.chan[channel].data32)
|
||||
#endif
|
||||
|
||||
// in order to avoid spikes, first set the value and then make an output
|
||||
// esp32 idf5 does not like this approach => output first, then value
|
||||
#define PIN_OUTPUT(pin, value) \
|
||||
{ \
|
||||
pinMode(pin, OUTPUT); \
|
||||
digitalWrite(pin, (value)); \
|
||||
}
|
||||
|
||||
#endif /* FAS_ARCH_COMMON_ESP32_IDF5_H */
|
||||
25
src/fas_arch/espidf_esp32.h
Normal file
25
src/fas_arch/espidf_esp32.h
Normal file
@@ -0,0 +1,25 @@
|
||||
#ifndef FAS_ARCH_ESPIDF_ESP32_H
|
||||
#define FAS_ARCH_ESPIDF_ESP32_H
|
||||
|
||||
// esp32 specific includes
|
||||
#include <math.h>
|
||||
|
||||
// on espidf need to use portDISABLE/ENABLE_INTERRUPTS
|
||||
#include <freertos/FreeRTOS.h>
|
||||
#include <freertos/task.h>
|
||||
#define fasDisableInterrupts portDISABLE_INTERRUPTS
|
||||
#define fasEnableInterrupts portENABLE_INTERRUPTS
|
||||
|
||||
// The espidf-platform needs a couple of arduino like definitions
|
||||
#define LOW 0
|
||||
#define HIGH 1
|
||||
#define OUTPUT GPIO_MODE_OUTPUT
|
||||
#define pinMode(pin, mode) \
|
||||
gpio_set_direction((gpio_num_t)pin, \
|
||||
(mode) == OUTPUT ? GPIO_MODE_OUTPUT : GPIO_MODE_INPUT)
|
||||
#define digitalWrite(pin, level) gpio_set_level((gpio_num_t)pin, level)
|
||||
#define digitalRead(pin) gpio_get_level((gpio_num_t)pin)
|
||||
|
||||
#include "fas_arch/common_esp32.h"
|
||||
|
||||
#endif /* FAS_ARCH_ESPIDF_ESP32_H */
|
||||
42
src/fas_arch/test_pc.h
Normal file
42
src/fas_arch/test_pc.h
Normal file
@@ -0,0 +1,42 @@
|
||||
#ifndef FAS_ARCH_TEST_PC_H
|
||||
#define FAS_ARCH_TEST_PC_H
|
||||
|
||||
// For pc-based testing like to have assert-macro
|
||||
#include <assert.h>
|
||||
|
||||
// and some more includes
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "../extras/tests/pc_based/stubs.h"
|
||||
|
||||
// For pc-based testing, the macro TEST is defined. The pc-based testing does
|
||||
// not support the concept of interrupts, so provide an empty definition
|
||||
#define fasEnableInterrupts()
|
||||
#define fasDisableInterrupts()
|
||||
|
||||
// The TEST target needs a couple of arduino like definitions
|
||||
#define LOW 0
|
||||
#define HIGH 1
|
||||
|
||||
// queue definitions for pc based testing
|
||||
#define MAX_STEPPER 2
|
||||
#define NUM_QUEUES 2
|
||||
#define fas_queue_A fas_queue[0]
|
||||
#define fas_queue_B fas_queue[1]
|
||||
#define QUEUE_LEN 16
|
||||
|
||||
// timing definitions for pc-based testing
|
||||
#define TICKS_PER_S 16000000L
|
||||
#define MIN_CMD_TICKS (TICKS_PER_S / 5000)
|
||||
#define MIN_DIR_DELAY_US (MIN_CMD_TICKS / (TICKS_PER_S / 1000000))
|
||||
#define MAX_DIR_DELAY_US (65535 / (TICKS_PER_S / 1000000))
|
||||
#define DELAY_MS_BASE 1
|
||||
#define SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING 0
|
||||
|
||||
#define noop_or_wait
|
||||
|
||||
#define SUPPORT_QUEUE_ENTRY_END_POS_U16
|
||||
|
||||
#endif /* FAS_ARCH_TEST_PC_H */
|
||||
56
src/fas_arch/test_probe.h
Normal file
56
src/fas_arch/test_probe.h
Normal file
@@ -0,0 +1,56 @@
|
||||
#ifndef TEST_PROBE_H
|
||||
#define TEST_PROBE_H
|
||||
// #define TEST_MODE
|
||||
// test mode in rmt:
|
||||
// code to investigate rmt module functioning
|
||||
|
||||
// #define ESP32_TEST_PROBE
|
||||
// #define ESP32C3_TEST_PROBE
|
||||
|
||||
// in rmt:
|
||||
// TEST_PROBE_1 on startQueue and queue stop, with double toggle at
|
||||
// startQueue TEST_PROBE_2 end interrupt, when rmt transmission hits buffer
|
||||
// end TEST_PROBE_3 threshold interrupt, after first buffer half transmission
|
||||
// is complete
|
||||
|
||||
#ifdef ESP32_TEST_PROBE
|
||||
#define TEST_PROBE_1 18
|
||||
#define TEST_PROBE_2 5
|
||||
#define TEST_PROBE_3 4
|
||||
#endif
|
||||
|
||||
#ifdef ESP32C3_TEST_PROBE
|
||||
#define TEST_PROBE_1 1
|
||||
#define TEST_PROBE_2 2
|
||||
#define TEST_PROBE_3 3
|
||||
#endif
|
||||
|
||||
#ifdef TEST_PROBE_1
|
||||
#define PROBE_1(x) digitalWrite(TEST_PROBE_1, x)
|
||||
#define PROBE_1_TOGGLE \
|
||||
pinMode(TEST_PROBE_1, OUTPUT); \
|
||||
digitalWrite(TEST_PROBE_1, digitalRead(TEST_PROBE_1) == HIGH ? LOW : HIGH)
|
||||
#else
|
||||
#define PROBE_1(x)
|
||||
#define PROBE_1_TOGGLE
|
||||
#endif
|
||||
#ifdef TEST_PROBE_2
|
||||
#define PROBE_2(x) digitalWrite(TEST_PROBE_2, x)
|
||||
#define PROBE_2_TOGGLE \
|
||||
pinMode(TEST_PROBE_2, OUTPUT); \
|
||||
digitalWrite(TEST_PROBE_2, digitalRead(TEST_PROBE_2) == HIGH ? LOW : HIGH)
|
||||
#else
|
||||
#define PROBE_2(x)
|
||||
#define PROBE_2_TOGGLE
|
||||
#endif
|
||||
#ifdef TEST_PROBE_3
|
||||
#define PROBE_3(x) digitalWrite(TEST_PROBE_3, x)
|
||||
#define PROBE_3_TOGGLE \
|
||||
pinMode(TEST_PROBE_3, OUTPUT); \
|
||||
digitalWrite(TEST_PROBE_3, digitalRead(TEST_PROBE_3) == HIGH ? LOW : HIGH)
|
||||
#else
|
||||
#define PROBE_3(x)
|
||||
#define PROBE_3_TOGGLE
|
||||
#endif
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user