Mirrors_Framework/g2
1
0
Fork 0
mirror of https://github.com/synthetos/g2.git synced 2026-02-06 02:51:54 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity
Files
5b0aa6be8a7239842c70b0fa13dd44371bc50236
g2/g2core/stepper.cpp

1152 lines
44 KiB
C++
Raw Blame History

/*
* stepper.cpp - stepper motor controls
* This file is part of the g2core project
*
* Copyright (c) 2010 - 2016 Alden S. Hart, Jr.
* Copyright (c) 2013 - 2016 Robert Giseburt
*
* This file ("the software") is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License, version 2 as published by the
* Free Software Foundation. You should have received a copy of the GNU General Public
* License, version 2 along with the software. If not, see <http://www.gnu.org/licenses/>.
*
* As a special exception, you may use this file as part of a software library without
* restriction. Specifically, if other files instantiate templates or use macros or
* inline functions from this file, or you compile this file and link it with other
* files to produce an executable, this file does not by itself cause the resulting
* executable to be covered by the GNU General Public License. This exception does not
* however invalidate any other reasons why the executable file might be covered by the
* GNU General Public License.
*
* THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY
* WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT
* SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF
* OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
/* This module provides the low-level stepper drivers and some related functions.
* See stepper.h for a detailed explanation of this module.
*/
#include "g2core.h"
#include "config.h"
#include "stepper.h"
#include "encoder.h"
#include "planner.h"
#include "hardware.h"
#include "text_parser.h"
#include "util.h"
#include "controller.h"
#include "xio.h"
/**** Debugging output with semihosting ****/
#include "MotateDebug.h"
// Unless debugging, this should always read "#if 0 && ..."
// DON'T COMMIT with anything else!
#if 0 && (IN_DEBUGGER == 1)
template<int32_t len>
void stepper_debug(const char (&str)[len]) { Motate::debug.write(str); };
#else
template<int32_t len>
void stepper_debug(const char (&str)[len]) { ; };
#endif
/**** Allocate structures ****/
stConfig_t st_cfg;
stPrepSingleton_t st_pre;
static stRunSingleton_t st_run HOT_DATA;
/**** Static functions ****/
static void _load_move(void);
// handy macro
//#define _f_to_period(f) (uint16_t)((float)F_CPU / (float)f)
/**** Setup motate ****/
using namespace Motate;
extern OutputPin<kDebug1_PinNumber> debug_pin1;
extern OutputPin<kDebug2_PinNumber> debug_pin2;
extern OutputPin<kDebug3_PinNumber> debug_pin3;
//extern OutputPin<kDebug4_PinNumber> debug_pin4;
dda_timer_type dda_timer {kTimerUpToMatch, FREQUENCY_DDA}; // stepper pulse generation
exec_timer_type exec_timer; // triggers calculation of next+1 stepper segment
fwd_plan_timer_type fwd_plan_timer; // triggers planning of next block
// SystickEvent for handling dweels (must be registered before it is active)
Motate::SysTickEvent dwell_systick_event {[&] {
if (--st_run.dwell_ticks_downcount == 0) {
SysTickTimer.unregisterEvent(&dwell_systick_event);
_load_move(); // load the next move at the current interrupt level
}
}, nullptr};
/************************************************************************************
**** CODE **************************************************************************
************************************************************************************/
/*
* stepper_init() - initialize stepper motor subsystem
* stepper_reset() - reset stepper motor subsystem
*
* Notes:
* - This init requires sys_init() to be run beforehand
* - microsteps are setup during config_init()
* - motor polarity is setup during config_init()
* - high level interrupts must be enabled in main() once all inits are complete
*/
/* NOTE: This is the bare code that the Motate timer calls replace.
* NB: requires: #include <component_tc.h>
*
* REG_TC1_WPMR = 0x54494D00; // enable write to registers
* TC_Configure(TC_BLOCK_DDA, TC_CHANNEL_DDA, TC_CMR_DDA);
* REG_RC_DDA = TC_RC_DDA; // set frequency
* REG_IER_DDA = TC_IER_DDA; // enable interrupts
* NVIC_EnableIRQ(TC_IRQn_DDA);
* pmc_enable_periph_clk(TC_ID_DDA);
* TC_Start(TC_BLOCK_DDA, TC_CHANNEL_DDA);
*/
void stepper_init()
{
memset(&st_run, 0, sizeof(st_run)); // clear all values, pointers and status
memset(&st_pre, 0, sizeof(st_pre)); // clear all values, pointers and status
stepper_init_assertions();
// setup DDA timer
// Longer duty cycles stretch ON pulses but 75% is about the upper limit and about
// optimal for 200 KHz DDA clock before the time in the OFF cycle is too short.
// If you need more pulse width you need to drop the DDA clock rate
dda_timer.setInterrupts(kInterruptOnOverflow | kInterruptPriorityHighest);
// setup software interrupt exec timer & initial condition
exec_timer.setInterrupts(kInterruptOnSoftwareTrigger | kInterruptPriorityHigh);
st_pre.buffer_state = PREP_BUFFER_OWNED_BY_EXEC;
// setup software interrupt forward plan timer & initial condition
fwd_plan_timer.setInterrupts(kInterruptOnSoftwareTrigger | kInterruptPriorityMedium);
// setup motor power levels and apply power level to stepper drivers
for (uint8_t motor=0; motor<MOTORS; motor++) {
Motors[motor]->setPowerLevel(st_cfg.mot[motor].power_level);
st_run.mot[motor].power_level_dynamic = st_cfg.mot[motor].power_level;
}
board_stepper_init();
stepper_reset(); // reset steppers to known state
}
/*
* stepper_reset() - reset stepper internals
*
* Used to initialize stepper and also to halt movement
*/
void stepper_reset()
{
dda_timer.stop(); // stop all movement
st_run.dda_ticks_downcount = 0; // signal the runtime is not busy
st_run.dwell_ticks_downcount = 0;
st_pre.buffer_state = PREP_BUFFER_OWNED_BY_EXEC; // set to EXEC or it won't restart
for (uint8_t motor=0; motor<MOTORS; motor++) {
st_pre.mot[motor].prev_direction = STEP_INITIAL_DIRECTION;
st_pre.mot[motor].direction = STEP_INITIAL_DIRECTION;
st_run.mot[motor].substep_accumulator = 0; // will become max negative during per-motor setup;
st_pre.mot[motor].corrected_steps = 0; // diagnostic only - no action effect
}
mp_set_steps_to_runtime_position(); // reset encoder to agree with the above
}
/*
* stepper_init_assertions() - test assertions, return error code if violation exists
* stepper_test_assertions() - test assertions, return error code if violation exists
*/
void stepper_init_assertions()
{
st_run.magic_end = MAGICNUM;
st_run.magic_start = MAGICNUM;
st_pre.magic_end = MAGICNUM;
st_pre.magic_start = MAGICNUM;
}
stat_t stepper_test_assertions()
{
if ((BAD_MAGIC(st_run.magic_start)) || (BAD_MAGIC(st_run.magic_end)) ||
(BAD_MAGIC(st_pre.magic_start)) || (BAD_MAGIC(st_pre.magic_end))) {
return(cm_panic(STAT_STEPPER_ASSERTION_FAILURE, "stepper_test_assertions()"));
}
return (STAT_OK);
}
/*
* st_runtime_isbusy() - return TRUE if runtime is busy:
*
* Busy conditions:
* - motors are running
* - dwell is running
*/
bool st_runtime_isbusy()
{
return (st_run.dda_ticks_downcount || st_run.dwell_ticks_downcount); // returns false if down count is zero
}
/*
* st_clc() - clear counters
*/
stat_t st_clc(nvObj_t *nv) // clear diagnostic counters, reset stepper prep
{
stepper_reset();
return(STAT_OK);
}
/*
* st_motor_power_callback() - callback to manage motor power sequencing
*
* Handles motor power-down timing, low-power idle, and adaptive motor power
*/
stat_t st_motor_power_callback() // called by controller
{
if (!mp_is_phat_city_time()) { // don't process this if you are time constrained in the planner
return (STAT_NOOP);
}
bool have_actually_stopped = false;
if ((!st_runtime_isbusy()) &&
(st_pre.buffer_state != PREP_BUFFER_OWNED_BY_LOADER) &&
(cm_get_cycle_state() == CYCLE_OFF)
)
{ // if there are no moves to load...
have_actually_stopped = true;
}
// manage power for each motor individually
for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) {
Motors[motor]->periodicCheck(have_actually_stopped);
}
return (STAT_OK);
}
/******************************
* Interrupt Service Routines *
******************************/
/***** Stepper Interrupt Service Routine ************************************************
* ISR - DDA timer interrupt routine - service ticks from DDA timer
*/
/*
* The DDA timer interrupt does this:
* - fire on overflow
* - clear interrupt condition
* - clear all step pins - this clears those the were set during the previous interrupt
* - if downcount == 0 and stop the timer and exit
* - run the DDA for each channel
* - decrement the downcount - if it reaches zero load the next segment
*
* Note that the motor_N.step.isNull() tests are compile-time tests, not run-time tests.
* If motor_N is not defined that if{} clause (i.e. that motor) drops out of the complied code.
*/
namespace Motate { // Must define timer interrupts inside the Motate namespace
template<>
void dda_timer_type::interrupt() HOT_FUNC;
template<>
void dda_timer_type::interrupt()
{
dda_timer.getInterruptCause(); // clear interrupt condition
// clear all steps from the previous interrupt
// for (uint8_t motor=0; motor<MOTORS; motor++) {
// Motors[motor]->stepEnd();
// }
// loop unrolled version (it's actually faster)
motor_1.stepEnd();
motor_2.stepEnd();
#if MOTORS > 2
motor_3.stepEnd();
#endif
#if MOTORS > 3
motor_4.stepEnd();
#endif
#if MOTORS > 4
motor_5.stepEnd();
#endif
#if MOTORS > 5
motor_6.stepEnd();
#endif
// process last DDA tick after end of segment
if (st_run.dda_ticks_downcount == 0) {
dda_timer.stop(); // turn it off or it will keep stepping out the last segment
return;
}
// The following code would work, but it's faster on the M3 to loop unroll it. Perhaps not on the M7
// for (uint8_t motor=0; motor<MOTORS; motor++) {
// if ((st_run.mot[motor].substep_accumulator += st_run.mot[motor].substep_increment) > 0) {
// Motors[motor]->stepStart(); // turn step bit on
// st_run.mot[motor].substep_accumulator -= st_run.dda_ticks_X_substeps;
// INCREMENT_ENCODER(motor);
// }
// }
// process DDAs for each motor
if ((st_run.mot[MOTOR_1].substep_accumulator += st_run.mot[MOTOR_1].substep_increment) > 0) {
motor_1.stepStart(); // turn step bit on
st_run.mot[MOTOR_1].substep_accumulator -= st_run.dda_ticks_X_substeps;
INCREMENT_ENCODER(MOTOR_1);
}
if ((st_run.mot[MOTOR_2].substep_accumulator += st_run.mot[MOTOR_2].substep_increment) > 0) {
motor_2.stepStart(); // turn step bit on
st_run.mot[MOTOR_2].substep_accumulator -= st_run.dda_ticks_X_substeps;
INCREMENT_ENCODER(MOTOR_2);
}
#if MOTORS > 2
if ((st_run.mot[MOTOR_3].substep_accumulator += st_run.mot[MOTOR_3].substep_increment) > 0) {
motor_3.stepStart(); // turn step bit on
st_run.mot[MOTOR_3].substep_accumulator -= st_run.dda_ticks_X_substeps;
INCREMENT_ENCODER(MOTOR_3);
}
#endif
#if MOTORS > 3
if ((st_run.mot[MOTOR_4].substep_accumulator += st_run.mot[MOTOR_4].substep_increment) > 0) {
motor_4.stepStart(); // turn step bit on
st_run.mot[MOTOR_4].substep_accumulator -= st_run.dda_ticks_X_substeps;
INCREMENT_ENCODER(MOTOR_4);
}
#endif
#if MOTORS > 4
if ((st_run.mot[MOTOR_5].substep_accumulator += st_run.mot[MOTOR_5].substep_increment) > 0) {
motor_5.stepStart(); // turn step bit on
st_run.mot[MOTOR_5].substep_accumulator -= st_run.dda_ticks_X_substeps;
INCREMENT_ENCODER(MOTOR_5);
}
#endif
#if MOTORS > 5
if ((st_run.mot[MOTOR_6].substep_accumulator += st_run.mot[MOTOR_6].substep_increment) > 0) {
motor_6.stepStart(); // turn step bit on
st_run.mot[MOTOR_6].substep_accumulator -= st_run.dda_ticks_X_substeps;
INCREMENT_ENCODER(MOTOR_6);
}
#endif
// Process end of segment.
// One more interrupt will occur to turn of any pulses set in this pass.
if (--st_run.dda_ticks_downcount == 0) {
_load_move(); // load the next move at the current interrupt level
}
} // MOTATE_TIMER_INTERRUPT
} // namespace Motate
/****************************************************************************************
* Exec sequencing code - computes and prepares next load segment
* st_request_exec_move() - SW interrupt to request to execute a move
* exec_timer interrupt - interrupt handler for calling exec function
*/
void st_request_exec_move()
{
stepper_debug("e");
exec_timer.setInterruptPending();
stepper_debug("!\n");
}
namespace Motate { // Define timer inside Motate namespace
template<>
void exec_timer_type::interrupt() HOT_FUNC;
template<>
void exec_timer_type::interrupt()
{
exec_timer.getInterruptCause(); // clears the interrupt condition
if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_EXEC) {
stepper_debug("E>");
if (mp_exec_move() != STAT_NOOP) {
stepper_debug("E+\n");
st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // flip it back
st_request_load_move();
return;
}
stepper_debug("E-\n");
}
}
} // namespace Motate
void st_request_forward_plan()
{
stepper_debug("p");
fwd_plan_timer.setInterruptPending();
}
namespace Motate { // Define timer inside Motate namespace
template<>
void fwd_plan_timer_type::interrupt() HOT_FUNC;
template<>
void fwd_plan_timer_type::interrupt()
{
fwd_plan_timer.getInterruptCause(); // clears the interrupt condition
stepper_debug("P>");
if (mp_forward_plan() != STAT_NOOP) { // We now have a move to exec.
stepper_debug("P+\n");
st_request_exec_move();
return;
}
stepper_debug("P-\n");
}
} // namespace Motate
/****************************************************************************************
* Loader sequencing code
* st_request_load_move() - fires a software interrupt (timer) to request to load a move
* load_move interrupt - interrupt handler for running the loader
*
* _load_move() can only be called be called from an ISR at the same or higher level as
* the DDA or dwell ISR. A software interrupt has been provided to allow a non-ISR to
* request a load (see st_request_load_move())
*/
void st_request_load_move()
{
if (st_runtime_isbusy()) { // don't request a load if the runtime is busy
return;
}
stepper_debug("l");
if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_LOADER) { // bother interrupting
stepper_debug("_");
_load_move();
}
}
/****************************************************************************************
* _load_move() - Dequeue move and load into stepper runtime structure
*
* This routine can only be called be called from an ISR at the same or
* higher level as the DDA or dwell ISR. A software interrupt has been
* provided to allow a non-ISR to request a load (st_request_load_move())
*
* In aline() code:
* - All axes must set steps and compensate for out-of-range pulse phasing.
* - If axis has 0 steps the direction setting can be omitted
* - If axis has 0 steps the motor power must be set accord to the power mode
*/
static void _load_move()
{
// Be aware that dda_ticks_downcount must equal zero for the loader to run.
// So the initial load must also have this set to zero as part of initialization
if (st_runtime_isbusy()) {
return; // exit if the runtime is busy
}
if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_LOADER) { // if there are no moves to load...
if (cm.motion_state == MOTION_RUN) {
#if IN_DEBUGGER == 1
//#warning debbugger REQUIRED for running this firmware!
// __asm__("BKPT"); // attempted to _load_move with PREP_BUFFER_OWNED_BY_EXEC and cm.motion_state == MOTION_RUN
#endif
st_request_exec_move();
return;
}
// ...start motor power timeouts
// for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) {
// Motors[motor]->motionStopped();
// }
// loop unrolled version
motor_1.motionStopped(); // ...start motor power timeouts
motor_2.motionStopped(); // ...start motor power timeouts
#if (MOTORS > 2)
motor_3.motionStopped(); // ...start motor power timeouts
#endif
#if (MOTORS > 3)
motor_4.motionStopped(); // ...start motor power timeouts
#endif
#if (MOTORS > 4)
motor_5.motionStopped(); // ...start motor power timeouts
#endif
#if (MOTORS > 5)
motor_6.motionStopped(); // ...start motor power timeouts
#endif
stepper_debug("•");
return;
} // if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_LOADER)
stepper_debug("^");
// handle aline loads first (most common case) NB: there are no more lines, only alines
if (st_pre.block_type == BLOCK_TYPE_ALINE) {
//**** setup the new segment ****
st_run.dda_ticks_downcount = st_pre.dda_ticks;
st_run.dda_ticks_X_substeps = st_pre.dda_ticks_X_substeps;
// INLINED VERSION: 4.3us
//**** MOTOR_1 LOAD ****
// These sections are somewhat optimized for execution speed. The whole load operation
// is supposed to take < 5 uSec (Arm M3 core). Be careful if you mess with this.
// the following if() statement sets the runtime substep increment value or zeroes it
if ((st_run.mot[MOTOR_1].substep_increment = st_pre.mot[MOTOR_1].substep_increment) != 0) {
// NB: If motor has 0 steps the following is all skipped. This ensures that state comparisons
// always operate on the last segment actually run by this motor, regardless of how many
// segments it may have been inactive in between.
// Apply accumulator correction if the time base has changed since previous segment
if (st_pre.mot[MOTOR_1].accumulator_correction_flag == true) {
st_pre.mot[MOTOR_1].accumulator_correction_flag = false;
st_run.mot[MOTOR_1].substep_accumulator *= st_pre.mot[MOTOR_1].accumulator_correction;
}
// Detect direction change and if so:
// Set the direction bit in hardware.
// Compensate for direction change by flipping substep accumulator value about its midpoint.
if (st_pre.mot[MOTOR_1].direction != st_pre.mot[MOTOR_1].prev_direction) {
st_pre.mot[MOTOR_1].prev_direction = st_pre.mot[MOTOR_1].direction;
st_run.mot[MOTOR_1].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_1].substep_accumulator);
motor_1.setDirection(st_pre.mot[MOTOR_1].direction);
}
// Enable the stepper and start/update motor power management
motor_1.enable();
SET_ENCODER_STEP_SIGN(MOTOR_1, st_pre.mot[MOTOR_1].step_sign);
} else { // Motor has 0 steps; might need to energize motor for power mode processing
motor_1.motionStopped();
}
// accumulate counted steps to the step position and zero out counted steps for the segment currently being loaded
ACCUMULATE_ENCODER(MOTOR_1);
#if (MOTORS >= 2)
if ((st_run.mot[MOTOR_2].substep_increment = st_pre.mot[MOTOR_2].substep_increment) != 0) {
if (st_pre.mot[MOTOR_2].accumulator_correction_flag == true) {
st_pre.mot[MOTOR_2].accumulator_correction_flag = false;
st_run.mot[MOTOR_2].substep_accumulator *= st_pre.mot[MOTOR_2].accumulator_correction;
}
if (st_pre.mot[MOTOR_2].direction != st_pre.mot[MOTOR_2].prev_direction) {
st_pre.mot[MOTOR_2].prev_direction = st_pre.mot[MOTOR_2].direction;
st_run.mot[MOTOR_2].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_2].substep_accumulator);
motor_2.setDirection(st_pre.mot[MOTOR_2].direction);
}
motor_2.enable();
SET_ENCODER_STEP_SIGN(MOTOR_2, st_pre.mot[MOTOR_2].step_sign);
} else {
motor_2.motionStopped();
}
ACCUMULATE_ENCODER(MOTOR_2);
#endif
#if (MOTORS >= 3)
if ((st_run.mot[MOTOR_3].substep_increment = st_pre.mot[MOTOR_3].substep_increment) != 0) {
if (st_pre.mot[MOTOR_3].accumulator_correction_flag == true) {
st_pre.mot[MOTOR_3].accumulator_correction_flag = false;
st_run.mot[MOTOR_3].substep_accumulator *= st_pre.mot[MOTOR_3].accumulator_correction;
}
if (st_pre.mot[MOTOR_3].direction != st_pre.mot[MOTOR_3].prev_direction) {
st_pre.mot[MOTOR_3].prev_direction = st_pre.mot[MOTOR_3].direction;
st_run.mot[MOTOR_3].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_3].substep_accumulator);
motor_3.setDirection(st_pre.mot[MOTOR_3].direction);
}
motor_3.enable();
SET_ENCODER_STEP_SIGN(MOTOR_3, st_pre.mot[MOTOR_3].step_sign);
} else {
motor_3.motionStopped();
}
ACCUMULATE_ENCODER(MOTOR_3);
#endif
#if (MOTORS >= 4)
if ((st_run.mot[MOTOR_4].substep_increment = st_pre.mot[MOTOR_4].substep_increment) != 0) {
if (st_pre.mot[MOTOR_4].accumulator_correction_flag == true) {
st_pre.mot[MOTOR_4].accumulator_correction_flag = false;
st_run.mot[MOTOR_4].substep_accumulator *= st_pre.mot[MOTOR_4].accumulator_correction;
}
if (st_pre.mot[MOTOR_4].direction != st_pre.mot[MOTOR_4].prev_direction) {
st_pre.mot[MOTOR_4].prev_direction = st_pre.mot[MOTOR_4].direction;
st_run.mot[MOTOR_4].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_4].substep_accumulator);
motor_4.setDirection(st_pre.mot[MOTOR_4].direction);
}
motor_4.enable();
SET_ENCODER_STEP_SIGN(MOTOR_4, st_pre.mot[MOTOR_4].step_sign);
} else {
motor_4.motionStopped();
}
ACCUMULATE_ENCODER(MOTOR_4);
#endif
#if (MOTORS >= 5)
if ((st_run.mot[MOTOR_5].substep_increment = st_pre.mot[MOTOR_5].substep_increment) != 0) {
if (st_pre.mot[MOTOR_5].accumulator_correction_flag == true) {
st_pre.mot[MOTOR_5].accumulator_correction_flag = false;
st_run.mot[MOTOR_5].substep_accumulator *= st_pre.mot[MOTOR_5].accumulator_correction;
}
if (st_pre.mot[MOTOR_5].direction != st_pre.mot[MOTOR_5].prev_direction) {
st_pre.mot[MOTOR_5].prev_direction = st_pre.mot[MOTOR_5].direction;
st_run.mot[MOTOR_5].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_5].substep_accumulator);
motor_5.setDirection(st_pre.mot[MOTOR_5].direction);
}
motor_5.enable();
SET_ENCODER_STEP_SIGN(MOTOR_5, st_pre.mot[MOTOR_5].step_sign);
} else {
motor_5.motionStopped();
}
ACCUMULATE_ENCODER(MOTOR_5);
#endif
#if (MOTORS >= 6)
if ((st_run.mot[MOTOR_6].substep_increment = st_pre.mot[MOTOR_6].substep_increment) != 0) {
if (st_pre.mot[MOTOR_6].accumulator_correction_flag == true) {
st_pre.mot[MOTOR_6].accumulator_correction_flag = false;
st_run.mot[MOTOR_6].substep_accumulator *= st_pre.mot[MOTOR_6].accumulator_correction;
}
if (st_pre.mot[MOTOR_6].direction != st_pre.mot[MOTOR_6].prev_direction) {
st_pre.mot[MOTOR_6].prev_direction = st_pre.mot[MOTOR_6].direction;
st_run.mot[MOTOR_6].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_6].substep_accumulator);
motor_6.setDirection(st_pre.mot[MOTOR_6].direction);
}
motor_6.enable();
SET_ENCODER_STEP_SIGN(MOTOR_6, st_pre.mot[MOTOR_6].step_sign);
} else {
motor_6.motionStopped();
}
ACCUMULATE_ENCODER(MOTOR_6);
#endif
//**** do this last ****
dda_timer.start(); // start the DDA timer if not already running
// handle dwells and commands
} else if (st_pre.block_type == BLOCK_TYPE_DWELL) {
st_run.dwell_ticks_downcount = st_pre.dwell_ticks;
// We now use SysTick events to handle dwells
SysTickTimer.registerEvent(&dwell_systick_event);
// handle synchronous commands
} else if (st_pre.block_type == BLOCK_TYPE_COMMAND) {
mp_runtime_command(st_pre.bf);
} // else null - which is okay in many cases
// all other cases drop to here (e.g. Null moves after Mcodes skip to here)
st_pre.block_type = BLOCK_TYPE_NULL;
st_pre.buffer_state = PREP_BUFFER_OWNED_BY_EXEC; // we are done with the prep buffer - flip the flag back
st_request_exec_move(); // exec and prep next move
}
/***********************************************************************************
* st_prep_line() - Prepare the next move for the loader
*
* This function does the math on the next pulse segment and gets it ready for
* the loader. It deals with all the DDA optimizations and timer setups so that
* loading can be performed as rapidly as possible. It works in joint space
* (motors) and it works in steps, not length units. All args are provided as
* floats and converted to their appropriate integer types for the loader.
*
* Args:
* - travel_steps[] are signed relative motion in steps for each motor. Steps are
* floats that typically have fractional values (fractional steps). The sign
* indicates direction. Motors that are not in the move should be 0 steps on input.
*
* - following_error[] is a vector of measured errors to the step count. Used for correction.
*
* - segment_time - how many minutes the segment should run. If timing is not
* 100% accurate this will affect the move velocity, but not the distance traveled.
*
* NOTE: Many of the expressions are sensitive to casting and execution order to avoid long-term
* accuracy errors due to floating point round off. One earlier failed attempt was:
* dda_ticks_X_substeps = (int32_t)((microseconds/1000000) * f_dda * dda_substeps);
*/
stat_t st_prep_line(float travel_steps[], float following_error[], float segment_time)
{
stepper_debug("😶");
// trap assertion failures and other conditions that would prevent queuing the line
if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_EXEC) { // never supposed to happen
return (cm_panic(STAT_INTERNAL_ERROR, "st_prep_line() prep sync error"));
} else if (isinf(segment_time)) { // never supposed to happen
return (cm_panic(STAT_PREP_LINE_MOVE_TIME_IS_INFINITE, "st_prep_line()"));
} else if (isnan(segment_time)) { // never supposed to happen
return (cm_panic(STAT_PREP_LINE_MOVE_TIME_IS_NAN, "st_prep_line()"));
// } else if (segment_time < EPSILON) {
// return (STAT_MINIMUM_TIME_MOVE);
}
// setup segment parameters
// - dda_ticks is the integer number of DDA clock ticks needed to play out the segment
// - ticks_X_substeps is the maximum depth of the DDA accumulator (as a negative number)
//st_pre.dda_period = _f_to_period(FREQUENCY_DDA); // FYI: this is a constant
st_pre.dda_ticks = (int32_t)(segment_time * 60 * FREQUENCY_DDA);// NB: converts minutes to seconds
st_pre.dda_ticks_X_substeps = st_pre.dda_ticks * DDA_SUBSTEPS;
// setup motor parameters
float correction_steps;
for (uint8_t motor=0; motor<MOTORS; motor++) { // remind us that this is motors, not axes
// Skip this motor if there are no new steps. Leave all other values intact.
if (fp_ZERO(travel_steps[motor])) {
st_pre.mot[motor].substep_increment = 0; // substep increment also acts as a motor flag
continue;
}
// Setup the direction, compensating for polarity.
// Set the step_sign which is used by the stepper ISR to accumulate step position
if (travel_steps[motor] >= 0) { // positive direction
st_pre.mot[motor].direction = DIRECTION_CW ^ st_cfg.mot[motor].polarity;
st_pre.mot[motor].step_sign = 1;
} else {
st_pre.mot[motor].direction = DIRECTION_CCW ^ st_cfg.mot[motor].polarity;
st_pre.mot[motor].step_sign = -1;
}
// Detect segment time changes and setup the accumulator correction factor and flag.
// Putting this here computes the correct factor even if the motor was dormant for some
// number of previous moves. Correction is computed based on the last segment time actually used.
if (fabs(segment_time - st_pre.mot[motor].prev_segment_time) > 0.0000001) { // highly tuned FP != compare
if (fp_NOT_ZERO(st_pre.mot[motor].prev_segment_time)) { // special case to skip first move
st_pre.mot[motor].accumulator_correction_flag = true;
st_pre.mot[motor].accumulator_correction = segment_time / st_pre.mot[motor].prev_segment_time;
}
st_pre.mot[motor].prev_segment_time = segment_time;
}
// 'Nudge' correction strategy. Inject a single, scaled correction value then hold off
// NOTE: This clause can be commented out to test for numerical accuracy and accumulating errors
if ((--st_pre.mot[motor].correction_holdoff < 0) &&
(fabs(following_error[motor]) > STEP_CORRECTION_THRESHOLD)) {
st_pre.mot[motor].correction_holdoff = STEP_CORRECTION_HOLDOFF;
correction_steps = following_error[motor] * STEP_CORRECTION_FACTOR;
if (correction_steps > 0) {
correction_steps = min3(correction_steps, fabs(travel_steps[motor]), STEP_CORRECTION_MAX);
} else {
correction_steps = max3(correction_steps, -fabs(travel_steps[motor]), -STEP_CORRECTION_MAX);
}
st_pre.mot[motor].corrected_steps += correction_steps;
travel_steps[motor] -= correction_steps;
}
// Compute substeb increment. The accumulator must be *exactly* the incoming
// fractional steps times the substep multiplier or positional drift will occur.
// Rounding is performed to eliminate a negative bias in the uint32 conversion
// that results in long-term negative drift. (fabs/round order doesn't matter)
st_pre.mot[motor].substep_increment = round(fabs(travel_steps[motor] * DDA_SUBSTEPS));
}
st_pre.block_type = BLOCK_TYPE_ALINE;
st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready
stepper_debug("👍🏻");
return (STAT_OK);
}
/*
* st_prep_null() - Keeps the loader happy. Otherwise performs no action
*/
void st_prep_null()
{
st_pre.block_type = BLOCK_TYPE_NULL;
st_pre.buffer_state = PREP_BUFFER_OWNED_BY_EXEC; // signal that prep buffer is empty
}
/*
* st_prep_command() - Stage command to execution
*/
void st_prep_command(void *bf)
{
st_pre.block_type = BLOCK_TYPE_COMMAND;
st_pre.bf = (mpBuf_t *)bf;
st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready
}
/*
* st_prep_dwell() - Add a dwell to the move buffer
*/
void st_prep_dwell(float milliseconds)
{
st_pre.block_type = BLOCK_TYPE_DWELL;
// we need dwell_ticks to be at least 1
st_pre.dwell_ticks = std::max((uint32_t)((milliseconds/1000.0) * FREQUENCY_DWELL), 1UL);
st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready
}
/*
* st_request_out_of_band_dwell()
* (only usable while exec isn't running, e.g. in feedhold or stopped states...)
* add a dwell to the loader without going through the planner buffers
*/
void st_request_out_of_band_dwell(float milliseconds)
{
st_prep_dwell(milliseconds);
st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready
st_request_load_move();
}
/*
* _set_hw_microsteps() - set microsteps in hardware
*/
static void _set_hw_microsteps(const uint8_t motor, const uint8_t microsteps)
{
if (motor >= MOTORS) {return;}
Motors[motor]->setMicrosteps(microsteps);
}
/***********************************************************************************
* CONFIGURATION AND INTERFACE FUNCTIONS
* Functions to get and set variables from the cfgArray table
***********************************************************************************/
/* HELPERS
* _get_motor() - helper to return motor number as an index or -1 if na
*/
static int8_t _get_motor(const index_t index)
{
char *ptr;
char motors[] = {"123456"};
char tmp[GROUP_LEN+1];
strcpy(tmp, cfgArray[index].group);
if ((ptr = strchr(motors, tmp[0])) == NULL) {
return (-1);
}
return (ptr - motors);
}
/*
* _set_motor_steps_per_unit() - what it says
* This function will need to be rethought if microstep morphing is implemented
*/
static void _set_motor_steps_per_unit(nvObj_t *nv)
{
uint8_t m = _get_motor(nv->index);
st_cfg.mot[m].units_per_step = (st_cfg.mot[m].travel_rev * st_cfg.mot[m].step_angle) / (360 * st_cfg.mot[m].microsteps);
st_cfg.mot[m].steps_per_unit = 1/st_cfg.mot[m].units_per_step;
}
/* PER-MOTOR FUNCTIONS
* st_set_ma() - map motor to axis
* st_set_sa() - set motor step angle
* st_set_tr() - set travel per motor revolution
* st_set_mi() - set motor microsteps
* st_set_pm() - set motor power mode
* st_get_pm() - get motor power mode
* st_set_pl() - set motor power level
*/
stat_t st_set_ma(nvObj_t *nv) // map motor to axis
{
if (nv->value < 0) {
nv->valuetype = TYPE_NULL;
return (STAT_INPUT_LESS_THAN_MIN_VALUE);
}
if (nv->value >= AXES) {
nv->valuetype = TYPE_NULL;
return (STAT_INPUT_EXCEEDS_MAX_VALUE);
}
set_ui8(nv);
return(STAT_OK);
}
stat_t st_set_sa(nvObj_t *nv) // motor step angle
{
if (nv->value <= 0) {
nv->valuetype = TYPE_NULL;
return (STAT_INPUT_LESS_THAN_MIN_VALUE);
}
if (nv->value >= 360) {
nv->valuetype = TYPE_NULL;
return (STAT_INPUT_EXCEEDS_MAX_VALUE);
}
set_flt(nv);
_set_motor_steps_per_unit(nv);
return(STAT_OK);
}
stat_t st_set_tr(nvObj_t *nv) // motor travel per revolution
{
if (nv->value <= 0) {
nv->valuetype = TYPE_NULL;
return (STAT_INPUT_LESS_THAN_MIN_VALUE);
}
set_flu(nv);
_set_motor_steps_per_unit(nv);
return(STAT_OK);
}
stat_t st_set_mi(nvObj_t *nv) // motor microsteps
{
if (nv->value <= 0) {
nv->valuetype = TYPE_NULL;
return (STAT_INPUT_LESS_THAN_MIN_VALUE);
}
uint8_t mi = (uint8_t)nv->value;
if ((mi != 1) && (mi != 2) && (mi != 4) && (mi != 8) && (mi != 16) && (mi != 32)) {
nv_add_conditional_message((const char *)"*** WARNING *** Setting non-standard microstep value");
}
set_ui8(nv); // set it anyway, even if it's unsupported
_set_motor_steps_per_unit(nv);
_set_hw_microsteps(_get_motor(nv->index), (uint8_t)nv->value);
return (STAT_OK);
}
stat_t st_get_su(nvObj_t *nv) // motor steps per unit (direct)
{
uint8_t m = _get_motor(nv->index);
nv->value = st_cfg.mot[m].steps_per_unit;
nv->valuetype = TYPE_FLOAT;
nv->precision = cfgArray[nv->index].precision;
return(STAT_OK);
}
stat_t st_set_su(nvObj_t *nv) // motor steps per unit (direct)
{
// Don't set a zero or negative value - just calculate based on sa, tr, and mi
// This way, if STEPS_PER_UNIT is set to 0 it is unused and we get the computed value
uint8_t m = _get_motor(nv->index);
if(nv->value <= 0) {
nv->value = st_cfg.mot[m].steps_per_unit;
_set_motor_steps_per_unit(nv);
return(STAT_OK);
}
// Do unit conversion here because it's a reciprocal value (rather than process_incoming_float())
if (cm_get_units_mode(MODEL) == INCHES) {
if (cm_get_axis_type(nv->index) == AXIS_TYPE_LINEAR) {
nv->value *= INCHES_PER_MM;
}
}
set_flt(nv);
st_cfg.mot[m].units_per_step = 1.0/st_cfg.mot[m].steps_per_unit;
// Scale TR so all the other values make sense
// You could scale any one of the other values, but TR makes the most sense
st_cfg.mot[m].travel_rev = (360.0*st_cfg.mot[m].microsteps)/(st_cfg.mot[m].steps_per_unit*st_cfg.mot[m].step_angle);
return(STAT_OK);
}
stat_t st_set_pm(nvObj_t *nv) // set motor power mode
{
if (nv->value < 0) {
nv->valuetype = TYPE_NULL;
return (STAT_INPUT_LESS_THAN_MIN_VALUE);
}
if (nv->value >= MOTOR_POWER_MODE_MAX_VALUE) {
nv->valuetype = TYPE_NULL;
return (STAT_INPUT_EXCEEDS_MAX_VALUE);
}
uint8_t motor = _get_motor(nv->index);
if (motor > MOTORS) {
nv->valuetype = TYPE_NULL;
return STAT_INPUT_VALUE_RANGE_ERROR;
};
// We do this *here* in order for this to take effect immediately.
// setPowerMode() sets the value and also executes it.
Motors[motor]->setPowerMode((stPowerMode)nv->value);
return (STAT_OK);
}
stat_t st_get_pm(nvObj_t *nv) // get motor power mode
{
uint8_t motor = _get_motor(nv->index);
if (motor > MOTORS) {
nv->valuetype = TYPE_NULL;
return STAT_INPUT_VALUE_RANGE_ERROR;
};
nv->value = (float)Motors[motor]->getPowerMode();
nv->valuetype = TYPE_INT;
return (STAT_OK);
}
/*
* st_set_pl() - set motor power level
*
* Input value may vary from 0.000 to 1.000 The setting is scaled to allowable PWM range.
* This function sets both the scaled and dynamic power levels, and applies the
* scaled value to the vref.
*/
stat_t st_set_pl(nvObj_t *nv) // motor power level
{
if (nv->value < (float)0.0) {
nv->valuetype = TYPE_NULL;
return (STAT_INPUT_LESS_THAN_MIN_VALUE);
}
if (nv->value > (float)1.0) {
nv->valuetype = TYPE_NULL;
return (STAT_INPUT_EXCEEDS_MAX_VALUE);
}
set_flt(nv); // set power_setting value in the motor config struct (st)
uint8_t motor = _get_motor(nv->index);
st_cfg.mot[motor].power_level = nv->value;
st_run.mot[motor].power_level_dynamic = (st_cfg.mot[motor].power_level);
Motors[motor]->setPowerLevel(st_cfg.mot[motor].power_level);
return(STAT_OK);
}
/*
* st_get_pwr() - get current motor power
*
* Returns the current power level of the motor given it's enable/disable state
* Returns 0.0 if motor is de-energized or disabled
* Can be extended to report idle setback by changing getCurrentPowerLevel()
*/
stat_t st_get_pwr(nvObj_t *nv)
{
// this is kind of a hack to extract the motor number from the table
uint8_t motor = (cfgArray[nv->index].token[3] & 0x0F) - 1;
if (motor > MOTORS) { return STAT_INPUT_VALUE_RANGE_ERROR; };
nv->value = Motors[motor]->getCurrentPowerLevel(motor);
nv->valuetype = TYPE_FLOAT;
nv->precision = cfgArray[nv->index].precision;
return (STAT_OK);
}
/* GLOBAL FUNCTIONS (SYSTEM LEVEL)
*
* st_set_mt() - set global motor timeout in seconds
* st_set_me() - enable motor power
* st_set_md() - disable motor power
*
* Calling me or md with NULL will enable or disable all motors
* Setting a value of 0 will enable or disable all motors
* Setting a value from 1 to MOTORS will enable or disable that motor only
*/
stat_t st_set_mt(nvObj_t *nv)
{
if (nv->value < MOTOR_TIMEOUT_SECONDS_MIN) {
nv->valuetype = TYPE_NULL;
return (STAT_INPUT_LESS_THAN_MIN_VALUE);
}
if (nv->value > MOTOR_TIMEOUT_SECONDS_MAX) {
nv->valuetype = TYPE_NULL;
return (STAT_INPUT_EXCEEDS_MAX_VALUE);
}
st_cfg.motor_power_timeout = nv->value;
return (STAT_OK);
}
// Make sure this function is not part of initialization --> f00
// nv->value is seconds of timeout
stat_t st_set_me(nvObj_t *nv)
{
for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) {
Motors[motor]->enable(nv->value); // nv->value is the timeout or 0 for default
}
return (STAT_OK);
}
// Make sure this function is not part of initialization --> f00
// nv-value is motor to disable, or 0 for all motors
stat_t st_set_md(nvObj_t *nv)
{
if (nv->value < 0) {
nv->valuetype = TYPE_NULL;
return (STAT_INPUT_LESS_THAN_MIN_VALUE);
}
if (nv->value > MOTORS) {
nv->valuetype = TYPE_NULL;
return (STAT_INPUT_EXCEEDS_MAX_VALUE);
}
// de-energize all motors
if ((uint8_t)nv->value == 0) { // 0 means all motors
for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) {
Motors[motor]->disable();
}
} else { // otherwise it's just one motor
Motors[(uint8_t)nv->value -1]->disable();
}
return (STAT_OK);
}
/***********************************************************************************
* TEXT MODE SUPPORT
* Functions to print variables from the cfgArray table
***********************************************************************************/
#ifdef __TEXT_MODE
static const char msg_units0[] = " in"; // used by generic print functions
static const char msg_units1[] = " mm";
static const char msg_units2[] = " deg";
static const char *const msg_units[] = { msg_units0, msg_units1, msg_units2 };
#define DEGREE_INDEX 2
static const char fmt_me[] = "motors energized\n";
static const char fmt_md[] = "motors de-energized\n";
static const char fmt_mt[] = "[mt] motor idle timeout%14.2f seconds\n";
static const char fmt_0ma[] = "[%s%s] m%s map to axis%15d [0=X,1=Y,2=Z...]\n";
static const char fmt_0sa[] = "[%s%s] m%s step angle%20.3f%s\n";
static const char fmt_0tr[] = "[%s%s] m%s travel per revolution%10.4f%s\n";
static const char fmt_0mi[] = "[%s%s] m%s microsteps%16d [1,2,4,8,16,32]\n";
static const char fmt_0su[] = "[%s%s] m%s steps per unit %17.5f steps per%s\n";
static const char fmt_0po[] = "[%s%s] m%s polarity%18d [0=normal,1=reverse]\n";
static const char fmt_0pm[] = "[%s%s] m%s power management%10d [0=disabled,1=always on,2=in cycle,3=when moving]\n";
static const char fmt_0pl[] = "[%s%s] m%s motor power level%13.3f [0.000=minimum, 1.000=maximum]\n";
static const char fmt_pwr[] = "[%s%s] Motor %c power level:%12.3f\n";
void st_print_me(nvObj_t *nv) { text_print(nv, fmt_me);} // TYPE_NULL - message only
void st_print_md(nvObj_t *nv) { text_print(nv, fmt_md);} // TYPE_NULL - message only
void st_print_mt(nvObj_t *nv) { text_print(nv, fmt_mt);} // TYPE_FLOAT
static void _print_motor_int(nvObj_t *nv, const char *format)
{
sprintf(cs.out_buf, format, nv->group, nv->token, nv->group, (int)nv->value);
xio_writeline(cs.out_buf);
}
static void _print_motor_flt(nvObj_t *nv, const char *format)
{
sprintf(cs.out_buf, format, nv->group, nv->token, nv->group, nv->value);
xio_writeline(cs.out_buf);
}
static void _print_motor_flt_units(nvObj_t *nv, const char *format, uint8_t units)
{
sprintf(cs.out_buf, format, nv->group, nv->token, nv->group, nv->value, GET_TEXT_ITEM(msg_units, units));
xio_writeline(cs.out_buf);
}
static void _print_motor_pwr(nvObj_t *nv, const char *format)
{
sprintf(cs.out_buf, format, nv->group, nv->token, nv->token[0], nv->value);
xio_writeline(cs.out_buf);
}
void st_print_ma(nvObj_t *nv) { _print_motor_int(nv, fmt_0ma);}
void st_print_sa(nvObj_t *nv) { _print_motor_flt_units(nv, fmt_0sa, DEGREE_INDEX);}
void st_print_tr(nvObj_t *nv) { _print_motor_flt_units(nv, fmt_0tr, cm_get_units_mode(MODEL));}
void st_print_mi(nvObj_t *nv) { _print_motor_int(nv, fmt_0mi);}
void st_print_su(nvObj_t *nv) { _print_motor_flt_units(nv, fmt_0su, cm_get_units_mode(MODEL));}
void st_print_po(nvObj_t *nv) { _print_motor_int(nv, fmt_0po);}
void st_print_pm(nvObj_t *nv) { _print_motor_int(nv, fmt_0pm);}
void st_print_pl(nvObj_t *nv) { _print_motor_flt(nv, fmt_0pl);}
void st_print_pwr(nvObj_t *nv){ _print_motor_pwr(nv, fmt_pwr);}
#endif // __TEXT_MODE
Reference in New Issue View Git Blame Copy Permalink