NVIC_SetPriority(TIMER1_IRQn, 1);
NVIC_SetPriority(TIMER2_IRQn, 4);
NVIC_SetPriority(PendSV_IRQn, 3);
- NVIC_SetPriority(RIT_IRQn, 3); // we make acceleration tick the same prio as pendsv so it can't be pre-empted by end of block
// Set other priorities lower than the timers
NVIC_SetPriority(ADC_IRQn, 5);
this->acceleration_ticks_per_second = THEKERNEL->config->value(acceleration_ticks_per_second_checksum)->by_default(1000)->as_number();
// Configure the step ticker ( TODO : shouldnt this go into stepticker's code ? )
- this->step_ticker->set_reset_delay( microseconds_per_step_pulse );
+ this->step_ticker->set_unstep_time( microseconds_per_step_pulse );
this->step_ticker->set_frequency( this->base_stepping_frequency );
- this->step_ticker->set_acceleration_ticks_per_second(acceleration_ticks_per_second); // must be set after set_frequency
// Core modules
this->add_module( this->gcode_dispatch = new GcodeDispatch() );
-/*
- This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
- Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
- Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
- You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
-*/
-
-
-#include "StepTicker.h"
-
-#include "libs/nuts_bolts.h"
-#include "libs/Module.h"
-#include "libs/Kernel.h"
-#include "StepperMotor.h"
-#include "StreamOutputPool.h"
-#include "system_LPC17xx.h" // mbed.h lib
-#include <math.h>
-#include <mri.h>
-
-#ifdef STEPTICKER_DEBUG_PIN
-#include "gpio.h"
-extern GPIO stepticker_debug_pin;
-#endif
-
-
-// StepTicker handles the base frequency ticking for the Stepper Motors / Actuators
-// It has a list of those, and calls their tick() functions at regular intervals
-// They then do Bresenham stuff themselves
-
-StepTicker* StepTicker::global_step_ticker;
-
-StepTicker::StepTicker(){
- StepTicker::global_step_ticker = this;
-
- // Configure the timer
- LPC_TIM0->MR0 = 10000000; // Initial dummy value for Match Register
- LPC_TIM0->MCR = 3; // Match on MR0, reset on MR0, match on MR1
- LPC_TIM0->TCR = 0; // Disable interrupt
-
- LPC_SC->PCONP |= (1 << 2); // Power Ticker ON
- LPC_TIM1->MR0 = 1000000;
- LPC_TIM1->MCR = 1;
- LPC_TIM1->TCR = 0; // Disable interrupt
-
- // Setup RIT timer
- LPC_SC->PCONP |= (1L<<16); // RIT Power
- LPC_SC->PCLKSEL1 &= ~(3L << 26); // Clear PCLK_RIT bits;
- LPC_SC->PCLKSEL1 |= (1L << 26); // Set PCLK_RIT bits to 0x01;
- LPC_RIT->RICOMPVAL = (uint32_t)(((SystemCoreClock / 1000000L) * 1000)-1); // 1ms period
- LPC_RIT->RICOUNTER = 0;
- // Set counter clear/reset after interrupt
- LPC_RIT->RICTRL |= (2L); //RITENCLR
- LPC_RIT->RICTRL &= ~(8L); // disable
- //NVIC_SetVector(RIT_IRQn, (uint32_t)&_ritisr);
-
- // Default start values
- this->a_move_finished = false;
- this->do_move_finished = 0;
- this->unstep.reset();
- this->set_frequency(100000);
- this->set_reset_delay(100);
- this->set_acceleration_ticks_per_second(1000);
- this->num_motors= 0;
- this->active_motor.reset();
- this->tick_cnt= 0;
-}
-
-StepTicker::~StepTicker() {
-}
-
-//called when everything is setup and interrupts can start
-void StepTicker::start() {
- NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler
- NVIC_EnableIRQ(TIMER1_IRQn); // Enable interrupt handler
- NVIC_EnableIRQ(RIT_IRQn);
-}
-
-// Set the base stepping frequency
-void StepTicker::set_frequency( float frequency ){
- this->frequency = frequency;
- this->period = floorf((SystemCoreClock/4.0F)/frequency); // SystemCoreClock/4 = Timer increments in a second
- LPC_TIM0->MR0 = this->period;
- if( LPC_TIM0->TC > LPC_TIM0->MR0 ){
- LPC_TIM0->TCR = 3; // Reset
- LPC_TIM0->TCR = 1; // Reset
- }
-}
-
-// Set the reset delay
-void StepTicker::set_reset_delay( float microseconds ){
- uint32_t delay = floorf((SystemCoreClock/4.0F)*(microseconds/1000000.0F)); // SystemCoreClock/4 = Timer increments in a second
- LPC_TIM1->MR0 = delay;
-}
-
-// this is the number of acceleration ticks per second
-void StepTicker::set_acceleration_ticks_per_second(uint32_t acceleration_ticks_per_second) {
- uint32_t us= roundf(1000000.0F/acceleration_ticks_per_second); // period in microseconds
- LPC_RIT->RICOMPVAL = (uint32_t)(((SystemCoreClock / 1000000L) * us)-1); // us
- LPC_RIT->RICOUNTER = 0;
- LPC_RIT->RICTRL |= (8L); // Enable rit
-}
-
-// Synchronize the acceleration timer, and optionally schedule it to fire now
-void StepTicker::synchronize_acceleration(bool fire_now) {
- LPC_RIT->RICOUNTER = 0;
- if(fire_now){
- NVIC_SetPendingIRQ(RIT_IRQn);
- }else{
- if(NVIC_GetPendingIRQ(RIT_IRQn)) {
- // clear pending interrupt so it does not interrupt immediately
- LPC_RIT->RICTRL |= 1L; // also clear the interrupt in case it fired
- NVIC_ClearPendingIRQ(RIT_IRQn);
- }
- }
-}
-
-
-// Call signal_move_finished() on each active motor that asked to be signaled. We do this instead of inside of tick() so that
-// all tick()s are called before we do the move finishing
-void StepTicker::signal_a_move_finished(){
- for (int motor = 0; motor < num_motors; motor++){
- if (this->active_motor[motor] && this->motor[motor]->is_move_finished){
- this->motor[motor]->signal_move_finished();
- // Theoretically this does nothing and the reason for it is currently unknown and/or forgotten
- // if(this->motor[motor]->moving == false){
- // if (motor > 0){
- // motor--;
- // bitmask >>= 1;
- // }
- // }
- }
- }
-}
-
-// Reset step pins on any motor that was stepped
-inline void StepTicker::unstep_tick(){
- for (int i = 0; i < num_motors; i++) {
- if(this->unstep[i]){
- this->motor[i]->unstep();
- }
- }
- this->unstep.reset();
-}
-
-extern "C" void TIMER1_IRQHandler (void){
- LPC_TIM1->IR |= 1 << 0;
- StepTicker::global_step_ticker->unstep_tick();
-}
-
-// The actual interrupt handler where we do all the work
-extern "C" void TIMER0_IRQHandler (void){
- StepTicker::global_step_ticker->TIMER0_IRQHandler();
-}
-
-extern "C" void RIT_IRQHandler (void){
- LPC_RIT->RICTRL |= 1L;
- StepTicker::global_step_ticker->acceleration_tick();
-}
-
-extern "C" void PendSV_Handler(void) {
- StepTicker::global_step_ticker->PendSV_IRQHandler();
-}
-
-// slightly lower priority than TIMER0, the whole end of block/start of block is done here allowing the timer to continue ticking
-void StepTicker::PendSV_IRQHandler (void) {
-
- if(this->do_move_finished.load() > 0) {
- this->do_move_finished--;
- #ifdef STEPTICKER_DEBUG_PIN
- stepticker_debug_pin= 1;
- #endif
-
- this->signal_a_move_finished();
-
- #ifdef STEPTICKER_DEBUG_PIN
- stepticker_debug_pin= 0;
- #endif
- }
-}
-
-// run in RIT lower priority than PendSV
-void StepTicker::acceleration_tick() {
- // call registered acceleration handlers
- for (size_t i = 0; i < acceleration_tick_handlers.size(); ++i) {
- acceleration_tick_handlers[i]();
- }
-}
-
-void StepTicker::TIMER0_IRQHandler (void){
- // Reset interrupt register
- LPC_TIM0->IR |= 1 << 0;
- tick_cnt++; // count number of ticks
-
- // Step pins NOTE takes 1.2us when nothing to step, 1.8-2us for one motor stepped and 2.6us when two motors stepped, 3.167us when three motors stepped
- for (uint32_t motor = 0; motor < num_motors; motor++){
- // send tick to all active motors
- if(this->active_motor[motor] && this->motor[motor]->tick()){
- // we stepped so schedule an unstep
- this->unstep[motor]= 1;
- }
- }
-
- // We may have set a pin on in this tick, now we reset the timer to set it off
- // Note there could be a race here if we run another tick before the unsteps have happened,
- // right now it takes about 3-4us but if the unstep were near 10uS or greater it would be an issue
- // also it takes at least 2us to get here so even when set to 1us pulse width it will still be about 3us
- if( this->unstep.any()){
- LPC_TIM1->TCR = 3;
- LPC_TIM1->TCR = 1;
- }
- // just let it run it will fire every 143 seconds
- // else{
- // LPC_TIM1->TCR = 0; // disable interrupt, no point in it running if nothing to do
- // }
-
- if(this->a_move_finished) {
- this->a_move_finished= false;
- this->do_move_finished++; // Note this is an atomic variable because it is updated in two interrupts of different priorities so can be pre-empted
- }
-
- // If a move finished in this tick, we have to tell the actuator to act accordingly
- if(this->do_move_finished.load() > 0){
- // we delegate the slow stuff to the pendsv handler which will run as soon as this interrupt exits
- //NVIC_SetPendingIRQ(PendSV_IRQn); this doesn't work
- SCB->ICSR = 0x10000000; // SCB_ICSR_PENDSVSET_Msk;
- }
-}
-
-// returns index of the stepper motor in the array and bitset
-int StepTicker::register_motor(StepperMotor* motor)
-{
- this->motor.push_back(motor);
- this->num_motors= this->motor.size();
- return this->num_motors-1;
-}
-
-// activate the specified motor, must have been registered
-void StepTicker::add_motor_to_active_list(StepperMotor* motor)
-{
- bool enabled= active_motor.any(); // see if interrupt was previously enabled
- active_motor[motor->index]= 1;
- if(!enabled) {
- LPC_TIM0->TCR = 1; // Enable interrupt
- }
-}
-
-// Remove a stepper from the list of active motors
-void StepTicker::remove_motor_from_active_list(StepperMotor* motor)
-{
- active_motor[motor->index]= 0;
- // If we have no motor to work on, disable the whole interrupt
- if(this->active_motor.none()){
- LPC_TIM0->TCR = 0; // Disable interrupt
- tick_cnt= 0;
- }
-}
+/*
+ This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
+ Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
+ Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+ You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include "StepTicker.h"
+
+#include "libs/nuts_bolts.h"
+#include "libs/Module.h"
+#include "libs/Kernel.h"
+#include "StepperMotor.h"
+#include "StreamOutputPool.h"
+#include "Block.h"
+
+#include "system_LPC17xx.h" // mbed.h lib
+#include <math.h>
+#include <mri.h>
+
+#ifdef STEPTICKER_DEBUG_PIN
+#include "gpio.h"
+extern GPIO stepticker_debug_pin;
+#endif
+
+StepTicker *StepTicker::instance;
+
+StepTicker::StepTicker()
+{
+ instance = this; // setup the Singleton instance of the stepticker
+
+ // Configure the timer
+ LPC_TIM0->MR0 = 10000000; // Initial dummy value for Match Register
+ LPC_TIM0->MCR = 3; // Match on MR0, reset on MR0, match on MR1
+ LPC_TIM0->TCR = 0; // Disable interrupt
+
+ LPC_SC->PCONP |= (1 << 2); // Power Ticker ON
+ LPC_TIM1->MR0 = 1000000;
+ LPC_TIM1->MCR = 1;
+ LPC_TIM1->TCR = 0; // Disable interrupt
+
+ // Default start values
+ this->set_frequency(100000);
+ this->set_unstep_time(100);
+
+ this->unstep.reset();
+ this->num_motors = 0;
+
+ this->move_issued = false;
+ this->next_block = nullptr;
+}
+
+StepTicker::~StepTicker()
+{
+}
+
+//called when everything is setup and interrupts can start
+void StepTicker::start()
+{
+ NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler
+ NVIC_EnableIRQ(TIMER1_IRQn); // Enable interrupt handler
+}
+
+// Set the base stepping frequency
+void StepTicker::set_frequency( float frequency )
+{
+ this->frequency = frequency;
+ this->period = floorf((SystemCoreClock / 4.0F) / frequency); // SystemCoreClock/4 = Timer increments in a second
+ LPC_TIM0->MR0 = this->period;
+ if( LPC_TIM0->TC > LPC_TIM0->MR0 ) {
+ LPC_TIM0->TCR = 3; // Reset
+ LPC_TIM0->TCR = 1; // Reset
+ }
+}
+
+// Set the reset delay
+void StepTicker::set_unstep_time( float microseconds )
+{
+ uint32_t delay = floorf((SystemCoreClock / 4.0F) * (microseconds / 1000000.0F)); // SystemCoreClock/4 = Timer increments in a second
+ LPC_TIM1->MR0 = delay;
+}
+
+// Reset step pins on any motor that was stepped
+void StepTicker::unstep_tick()
+{
+ for (int i = 0; i < num_motors; i++) {
+ if(this->unstep[i]) {
+ this->motor[i]->unstep();
+ }
+ }
+ this->unstep.reset();
+}
+
+extern "C" void TIMER1_IRQHandler (void)
+{
+ LPC_TIM1->IR |= 1 << 0;
+ StepTicker::getInstance()->unstep_tick();
+}
+
+// The actual interrupt handler where we do all the work
+extern "C" void TIMER0_IRQHandler (void)
+{
+ StepTicker::getInstance()->TIMER0_IRQHandler();
+}
+
+extern "C" void PendSV_Handler(void)
+{
+ StepTicker::getInstance()->PendSV_IRQHandler();
+}
+
+// slightly lower priority than TIMER0, the whole end of block/start of block is done here allowing the timer to continue ticking
+void StepTicker::PendSV_IRQHandler (void)
+{
+
+ if(this->do_move_finished.load() > 0) {
+ this->do_move_finished--;
+#ifdef STEPTICKER_DEBUG_PIN
+ stepticker_debug_pin = 1;
+#endif
+
+ // all moves finished signal block is finished
+ if(finished_fnc) finished_fnc();
+
+#ifdef STEPTICKER_DEBUG_PIN
+ stepticker_debug_pin = 0;
+#endif
+ }
+}
+
+
+// step clock
+void StepTicker::TIMER0_IRQHandler (void)
+{
+ static uint32_t current_tick = 0;
+
+ // Reset interrupt register
+ LPC_TIM0->IR |= 1 << 0;
+
+ if(!move_issued) return; // if nothing has been setup we ignore the ticks
+
+ current_tick++; // count number of ticks
+
+ bool still_moving = false;
+
+ // foreach motor, if it is active see if time to issue a step to that motor
+ for (uint8_t m = 0; m < num_motors; m++) {
+ if(tick_info[m].steps_to_move == 0) continue; // not active
+
+ still_moving = true;
+ tick_info[m].steps_per_tick += tick_info[m].acceleration_change;
+
+ if(current_tick == tick_info[m].next_accel_event) {
+ if(current_tick == block_info.accelerate_until) { // We are done accelerating, deceleration becomes 0 : plateau
+ tick_info[m].acceleration_change = 0;
+ if(block_info.decelerate_after < block_info.total_move_ticks) {
+ tick_info[m].next_accel_event = block_info.decelerate_after;
+ if(current_tick != block_info.decelerate_after) { // We start decelerating
+ tick_info[m].steps_per_tick = (tick_info[m].axis_ratio * block_info.maximum_rate) / frequency; // steps/sec / tick frequency to get steps per tick
+ }
+ }
+ }
+
+ if(current_tick == block_info.decelerate_after) { // We start decelerating
+ tick_info[m].acceleration_change = -block_info.deceleration_per_tick * tick_info[m].axis_ratio;
+ }
+ }
+
+ // protect against rounding errors and such
+ if(tick_info[m].steps_per_tick <= 0) {
+ tick_info[m].counter = 1.0F; // we complete this step
+ tick_info[m].steps_per_tick = 0;
+ }
+
+ tick_info[m].counter += tick_info[m].steps_per_tick;
+
+ if(tick_info[m].counter >= 1.0F) { // step time
+ tick_info[m].counter -= 1.0F;
+ ++tick_info[m].step_count;
+
+ // step the motor
+ motor[m]->step();
+ // we stepped so schedule an unstep
+ unstep.set(m);
+
+ if(tick_info[m].step_count == tick_info[m].steps_to_move) {
+ // done
+ tick_info[m].steps_to_move = 0;
+ }
+ }
+ }
+
+ // We may have set a pin on in this tick, now we reset the timer to set it off
+ // Note there could be a race here if we run another tick before the unsteps have happened,
+ // right now it takes about 3-4us but if the unstep were near 10uS or greater it would be an issue
+ // also it takes at least 2us to get here so even when set to 1us pulse width it will still be about 3us
+ if( unstep.any()) {
+ LPC_TIM1->TCR = 3;
+ LPC_TIM1->TCR = 1;
+ }
+
+ if(!still_moving) {
+ current_tick = 0;
+
+ // get next static block and tick info from next block
+ // do it here so there is no delay in ticks
+ if(next_block != nullptr) {
+ // copy data
+ copy_block(next_block);
+ next_block = nullptr;
+
+ } else {
+ move_issued = false; // nothing to do as no more blocks
+ }
+
+ // all moves finished
+ // we delegate the slow stuff to the pendsv handler which will run as soon as this interrupt exits
+ //NVIC_SetPendingIRQ(PendSV_IRQn); this doesn't work
+ SCB->ICSR = 0x10000000; // SCB_ICSR_PENDSVSET_Msk;
+ }
+}
+
+// called in ISR if running, else can be called from anything to start
+void StepTicker::copy_block(Block *block)
+{
+ block_info.accelerate_until = block->accelerate_until;
+ block_info.decelerate_after = block->decelerate_after;
+ block_info.maximum_rate = block->maximum_rate;
+ block_info.deceleration_per_tick = block->deceleration_per_tick;
+ block_info.total_move_ticks = block->total_move_ticks;
+
+ float inv = 1.0F / block->steps_event_count;
+ for (uint8_t m = 0; m < num_motors; m++) {
+ uint32_t steps = block->steps[m];
+ tick_info[m].steps_to_move = steps;
+ if(steps == 0) continue;
+
+ // set direction bit here
+ motor[m]->set_direction(block->direction_bits[m]);
+
+ float aratio = inv * steps;
+ tick_info[m].steps_per_tick = (block->initial_rate * aratio) / frequency; // steps/sec / tick frequency to get steps per tick; // 2.30 fixed point
+ tick_info[m].counter = 0; // 2.30 fixed point
+ tick_info[m].axis_ratio = aratio;
+ tick_info[m].step_count = 0;
+ tick_info[m].next_accel_event = block->total_move_ticks + 1;
+ tick_info[m].acceleration_change = 0;
+ if(block->accelerate_until != 0) { // If the next accel event is the end of accel
+ tick_info[m].next_accel_event = block->accelerate_until;
+ tick_info[m].acceleration_change = block->acceleration_per_tick;
+
+ } else if(block->decelerate_after == 0 /*&& block->accelerate_until == 0*/) {
+ // we start off decelerating
+ tick_info[m].acceleration_change = -block->deceleration_per_tick;
+
+ } else if(block->decelerate_after != block->total_move_ticks /*&& block->accelerate_until == 0*/) {
+ // If the next event is the start of decel ( don't set this if the next accel event is accel end )
+ tick_info[m].next_accel_event = block->decelerate_after;
+ }
+ tick_info[m].acceleration_change *= aratio;
+ }
+ move_issued = true;
+}
+
+// returns index of the stepper motor in the array and bitset
+int StepTicker::register_motor(StepperMotor* m)
+{
+ motor[num_motors++] = m;
+ return num_motors - 1;
+}
-#ifndef STEPTICKER_H
-#define STEPTICKER_H
+#pragma once
#include <stdint.h>
-#include <vector>
+#include <array>
#include <bitset>
#include <functional>
#include <atomic>
+#include "ActuatorCoordinates.h"
+
class StepperMotor;
+class Block;
class StepTicker{
public:
- static StepTicker* global_step_ticker;
-
StepTicker();
~StepTicker();
void set_frequency( float frequency );
void signal_a_move_finished();
- void set_reset_delay( float seconds );
+ void set_unstep_time( float microseconds );
int register_motor(StepperMotor* motor);
- void add_motor_to_active_list(StepperMotor* motor);
- void remove_motor_from_active_list(StepperMotor* motor);
- void set_acceleration_ticks_per_second(uint32_t acceleration_ticks_per_second);
float get_frequency() const { return frequency; }
void unstep_tick();
- uint32_t get_tick_cnt() const { return tick_cnt; }
- uint32_t ticks_since(uint32_t last) const { return (tick_cnt>=last) ? tick_cnt-last : (UINT32_MAX-last) + tick_cnt + 1; }
void TIMER0_IRQHandler (void);
void PendSV_IRQHandler (void);
- void register_acceleration_tick_handler(std::function<void(void)> cb){
- acceleration_tick_handlers.push_back(cb);
- }
- void acceleration_tick();
- void synchronize_acceleration(bool fire_now);
void start();
+ void copy_block(Block *block);
+ void set_next_block(Block *block) { next_block= block; }
+ bool is_next_block() const { return next_block != nullptr; }
- friend class StepperMotor;
+ // whatever setup the block should register this to know when it is done
+ std::function<void()> finished_fnc{nullptr};
+
+ static StepTicker *getInstance() { return instance; }
private:
+ static StepTicker *instance;
+
float frequency;
uint32_t period;
- volatile uint32_t tick_cnt;
- std::vector<std::function<void(void)>> acceleration_tick_handlers;
- std::vector<StepperMotor*> motor;
- std::bitset<32> active_motor; // limit to 32 motors
- std::bitset<32> unstep; // limit to 32 motors
+ std::array<StepperMotor*, k_max_actuators> motor;
std::atomic_uchar do_move_finished;
+ std::bitset<k_max_actuators> unstep;
- uint8_t num_motors;
- volatile bool a_move_finished;
-};
-
+ // this is tick info needed for this block. applies to all motors
+ struct block_info_t {
+ uint32_t accelerate_until;
+ uint32_t decelerate_after;
+ uint32_t maximum_rate;
+ uint32_t deceleration_per_tick;
+ uint32_t total_move_ticks;
+ };
+ block_info_t block_info;
+ Block *next_block{nullptr};
+ // this is the data needed to determine when each motor needs to be issued a step
+ struct tickinfo_t {
+ float steps_per_tick; // 2.30 fixed point
+ float counter; // 2.30 fixed point
+ float acceleration_change; // 1.30 fixed point signed
+ float axis_ratio;
+ uint32_t steps_to_move;
+ uint32_t step_count;
+ uint32_t next_accel_event;
+ };
+ std::array<tickinfo_t, k_max_actuators> tick_info;
-#endif
+ struct {
+ volatile bool move_issued:1;
+ uint8_t num_motors:4;
+ };
+};
-/*
- This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
- Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
- Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
- You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
-*/
-#include "StepperMotor.h"
-
-#include "Kernel.h"
-#include "MRI_Hooks.h"
-#include "StepTicker.h"
-
-#include <math.h>
-
-// in steps/sec the default minimum speed (was 20steps/sec hardcoded)
-float StepperMotor::default_minimum_actuator_rate= 20.0F;
-
-// A StepperMotor represents an actual stepper motor. It is used to generate steps that move the actual motor at a given speed
-
-StepperMotor::StepperMotor()
-{
- init();
-}
-
-StepperMotor::StepperMotor(Pin &step, Pin &dir, Pin &en) : step_pin(step), dir_pin(dir), en_pin(en)
-{
- init();
- enable(false);
- set_high_on_debug(en.port_number, en.pin);
-}
-
-StepperMotor::~StepperMotor()
-{
-}
-
-void StepperMotor::init()
-{
- // register this motor with the step ticker, and get its index in that array and bit position
- this->index= THEKERNEL->step_ticker->register_motor(this);
- this->moving = false;
- this->fx_counter = 0;
- this->fx_ticks_per_step = 0xFFFFF000UL; // some big number so we don't start stepping before it is set
- this->stepped = 0;
- this->steps_to_move = 0;
- this->is_move_finished = false;
- this->last_step_tick_valid= false;
- this->last_step_tick= 0;
- this->force_finish= false;
-
- steps_per_mm = 1.0F;
- max_rate = 50.0F;
- minimum_step_rate = default_minimum_actuator_rate;
-
- last_milestone_steps = 0;
- last_milestone_mm = 0.0F;
- current_position_steps= 0;
- signal_step= 0;
-}
-
-
-// This is called ( see the .h file, we had to put a part of things there for obscure inline reasons ) when a step has to be generated
-// we also here check if the move is finished etc ..
-// This is in highest priority interrupt so cannot be pre-empted
-void StepperMotor::step()
-{
- // ignore if we are still processing the end of a block
- if(this->is_move_finished) return;
-
- if(!this->force_finish) {
- // output to pins 37t
- this->step_pin.set( 1 );
-
- // move counter back 11t
- this->fx_counter -= this->fx_ticks_per_step;
-
- // we have moved a step 9t
- this->stepped++;
-
- // keep track of actuators actual position in steps
- this->current_position_steps += (this->direction ? -1 : 1);
-
- // we may need to callback on a specific step, usually used to synchronize deceleration timer
- if(this->signal_step != 0 && this->stepped == this->signal_step) {
- THEKERNEL->step_ticker->synchronize_acceleration(true);
- this->signal_step= 0;
- }
- }
-
- // Is this move finished ?
- if( this->force_finish || this->stepped == this->steps_to_move) {
- // Mark it as finished, then StepTicker will call signal_move_finished()
- // This is so we don't call that before all the steps have been generated for this tick()
- this->is_move_finished = true;
- THEKERNEL->step_ticker->a_move_finished= true;
- this->last_step_tick= THEKERNEL->step_ticker->get_tick_cnt(); // remember when last step was
- if(this->force_finish) this->steps_to_move = stepped;
- }
-}
-
-// If the move is finished, the StepTicker will call this ( because we asked it to in tick() )
-void StepperMotor::signal_move_finished()
-{
- // work is done ! 8t
- this->moving = false;
- this->steps_to_move = 0;
- this->minimum_step_rate = default_minimum_actuator_rate;
-
- // signal it to whatever cares
- // in this call a new block may start, new moves set and new speeds
- this->end_hook->call();
-
- // We only need to do this if we were not instructed to move
- if( !this->moving ) {
- this->update_exit_tick();
- }
-
- this->is_move_finished = false;
-}
-
-// This is just a way not to check for ( !this->moving || this->fx_ticks_per_step == 0 ) at every tick()
-void StepperMotor::update_exit_tick()
-{
- if( !this->moving || this->steps_to_move == 0 ) {
- // No more ticks will be received and no more events from StepTicker
- THEKERNEL->step_ticker->remove_motor_from_active_list(this);
- } else {
- // we will now get ticks and StepTIcker will send us events
- THEKERNEL->step_ticker->add_motor_to_active_list(this);
- }
-}
-
-// Instruct the StepperMotor to move a certain number of steps
-StepperMotor* StepperMotor::move( bool direction, unsigned int steps, float initial_speed)
-{
- this->dir_pin.set(direction);
- this->direction = direction;
- this->force_finish= false;
-
- // How many steps we have to move until the move is done
- this->steps_to_move = steps;
-
- // Zero our tool counters
- this->stepped = 0;
- this->fx_ticks_per_step = 0xFFFFF000UL; // some big number so we don't start stepping before it is set again
- if(this->last_step_tick_valid) {
- // we set this based on when the last step was, thus compensating for missed ticks
- uint32_t ts= THEKERNEL->step_ticker->ticks_since(this->last_step_tick);
- // if an axis stops too soon then we can get a huge number of ticks here which causes problems, so if the number of ticks is too great we ignore them
- // example of when this happens is when one axis is going very slow and the min 20steps/sec kicks in, the axis will reach its target much sooner leaving a long gap
- // until the end of the block.
- // TODO we may need to set this based on the current step rate, trouble is we don't know what that is yet, we could use the last fx_ticks_per_step as a guide
- if(ts > 5) ts= 5; // limit to 50us catch up around 1-2 steps
- else if(ts > 15) ts= 0; // no way to know what the delay was
- this->fx_counter= ts*fx_increment;
- }else{
- this->fx_counter = 0; // set to zero as there was no step last block
- }
-
- // Starting now we are moving
- if( steps > 0 ) {
- if(initial_speed >= 0.0F) set_speed(initial_speed);
- this->moving = true;
- } else {
- this->moving = false;
- }
- this->update_exit_tick();
- return this;
-}
-
-// Set the speed at which this stepper moves in steps/sec, should be called set_step_rate()
-// we need to make sure that we have a minimum speed here and that it fits the 32bit fixed point fx counters
-// Note nothing will really ever go as slow as the minimum speed here, it is just forced to avoid bad errors
-// fx_ticks_per_step is what actually sets the step rate, it is fixed point 18.14
-StepperMotor* StepperMotor::set_speed( float speed )
-{
- if(speed < minimum_step_rate) {
- speed= minimum_step_rate;
- }
-
- // if(speed <= 0.0F) { // we can't actually do 0 but we can get close, need to avoid divide by zero later on
- // this->fx_ticks_per_step= 0xFFFFFFFFUL; // 0.381 steps/sec
- // this->steps_per_second = THEKERNEL->step_ticker->get_frequency() / (this->fx_ticks_per_step >> fx_shift);
- // return;
- // }
-
- // How many steps we must output per second
- this->steps_per_second = speed;
-
- // set the new speed, NOTE this can be pre-empted by stepticker so the following write needs to be atomic
- this->fx_ticks_per_step= floor(fx_increment * THEKERNEL->step_ticker->get_frequency() / speed);
- return this;
-}
-
-void StepperMotor::change_steps_per_mm(float new_steps)
-{
- steps_per_mm = new_steps;
- last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm);
- current_position_steps = last_milestone_steps;
-}
-
-void StepperMotor::change_last_milestone(float new_milestone)
-{
- last_milestone_mm = new_milestone;
- last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm);
- current_position_steps = last_milestone_steps;
-}
-
-int StepperMotor::steps_to_target(float target)
-{
- int target_steps = lroundf(target * steps_per_mm);
- return target_steps - last_milestone_steps;
-}
+/*
+ This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
+ Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
+ Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+ You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
+*/
+#include "StepperMotor.h"
+
+#include "Kernel.h"
+#include "MRI_Hooks.h"
+#include "StepTicker.h"
+
+#include <math.h>
+
+// in steps/sec the default minimum speed (was 20steps/sec hardcoded)
+float StepperMotor::default_minimum_actuator_rate= 20.0F;
+
+// A StepperMotor represents an actual stepper motor. It is used to generate steps that move the actual motor at a given speed
+
+StepperMotor::StepperMotor()
+{
+ init();
+}
+
+StepperMotor::StepperMotor(Pin &step, Pin &dir, Pin &en) : step_pin(step), dir_pin(dir), en_pin(en)
+{
+ init();
+ enable(false);
+ set_high_on_debug(en.port_number, en.pin);
+}
+
+StepperMotor::~StepperMotor()
+{
+}
+
+void StepperMotor::init()
+{
+ // register this motor with the step ticker, and get its index in that array and bit position
+ this->index= THEKERNEL->step_ticker->register_motor(this);
+ this->moving = false;
+ this->stepped = 0;
+ this->is_move_finished = false;
+ this->force_finish= false;
+
+ steps_per_mm = 1.0F;
+ max_rate = 50.0F;
+
+ last_milestone_steps = 0;
+ last_milestone_mm = 0.0F;
+ current_position_steps= 0;
+}
+
+// // Instruct the StepperMotor to move a certain number of steps
+// StepperMotor* StepperMotor::move( bool direction, unsigned int steps, float initial_speed)
+// {
+// set_direction(direction);
+// this->direction = direction;
+// this->force_finish= false;
+
+// // Zero our tool counters
+// this->stepped = 0;
+// return this;
+// }
+
+void StepperMotor::change_steps_per_mm(float new_steps)
+{
+ steps_per_mm = new_steps;
+ last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm);
+ current_position_steps = last_milestone_steps;
+}
+
+void StepperMotor::change_last_milestone(float new_milestone)
+{
+ last_milestone_mm = new_milestone;
+ last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm);
+ current_position_steps = last_milestone_steps;
+}
+
+int StepperMotor::steps_to_target(float target)
+{
+ int target_steps = lroundf(target * steps_per_mm);
+ return target_steps - last_milestone_steps;
+}
#ifndef STEPPERMOTOR_H
#define STEPPERMOTOR_H
-#include "libs/Hook.h"
#include "Pin.h"
#include <atomic>
#include <functional>
StepperMotor(Pin& step, Pin& dir, Pin& en);
~StepperMotor();
- void step();
- inline void unstep() { step_pin.set(0); };
+ inline void step() { current_position_steps += (direction?-1:1); step_pin.set( 1 ); }
+ inline void unstep() { step_pin.set(0); }
+ inline void set_direction(bool f) { direction= f; dir_pin.set(f); }
inline void enable(bool state) { en_pin.set(!state); };
bool is_moving() const { return moving; }
bool which_direction() const { return direction; }
- void move_finished();
- StepperMotor* move( bool direction, unsigned int steps, float initial_speed= -1.0F);
- void signal_move_finished();
- StepperMotor* set_speed( float speed );
- void set_moved_last_block(bool flg) { last_step_tick_valid= flg; }
- void update_exit_tick();
+// void move_finished();
+// StepperMotor* move( bool direction, unsigned int steps, float initial_speed= -1.0F);
+// StepperMotor* set_speed( float speed );
float get_steps_per_second() const { return steps_per_second; }
float get_steps_per_mm() const { return steps_per_mm; }
float get_current_position(void) const { return (float)current_position_steps/steps_per_mm; }
float get_max_rate(void) const { return max_rate; }
void set_max_rate(float mr) { max_rate= mr; }
- float get_min_rate(void) const { return minimum_step_rate; }
- void set_min_rate(float mr) { minimum_step_rate= mr; }
int steps_to_target(float);
- uint32_t get_steps_to_move() const { return steps_to_move; }
uint32_t get_stepped() const { return stepped; }
void force_finish_move() { force_finish= true; }
- template<typename T> void attach( T *optr, uint32_t ( T::*fptr )( uint32_t ) ){
- Hook* hook = new Hook();
- hook->attach(optr, fptr);
- this->end_hook = hook;
- }
-
friend class StepTicker;
friend class Stepper;
friend class Planner;
void init();
int index;
- Hook* end_hook;
Pin step_pin;
Pin dir_pin;
int32_t last_milestone_steps;
float last_milestone_mm;
- uint32_t steps_to_move;
- uint32_t stepped;
- uint32_t last_step_tick;
- uint32_t signal_step;
-
- // set to 32 bit fixed point, 18:14 bits fractional
- static const uint32_t fx_shift= 14;
- static const uint32_t fx_increment= ((uint32_t)1<<fx_shift);
- uint32_t fx_counter;
- uint32_t fx_ticks_per_step;
-
+ uint32_t stepped; // TBD may not be needed
volatile struct {
volatile bool is_move_finished:1; // Whether the move just finished
volatile bool moving:1;
volatile bool force_finish:1; // set to force a move to finish early
bool direction:1;
- bool last_step_tick_valid:1; // set if the last step tick time is valid (ie the motor moved last block)
- };
-
- // Called a great many times per second, to step if we have to now
- inline bool tick() {
- // increase the ( 32 fixed point 18:14 ) counter by one tick 11t
- fx_counter += fx_increment;
-
- // if we are to step now
- if (fx_counter >= fx_ticks_per_step){
- step();
- return true;
- }
- return false;
};
};
kernel->add_module( new(AHB0) CurrentControl() );
kernel->add_module( new(AHB0) KillButton() );
kernel->add_module( new(AHB0) PlayLed() );
- kernel->add_module( new(AHB0) Endstops() );
-
// these modules can be completely disabled in the Makefile by adding to EXCLUDE_MODULES
+ #ifndef NO_TOOLS_ENDSTOPS
+ kernel->add_module( new(AHB0) Endstops() );
+ #endif
+
#ifndef NO_TOOLS_SWITCH
SwitchPool *sp= new SwitchPool();
sp->load_tools();
endif
# add any modules that you do not want included in the build
-EXCLUDE_MODULES = tools/touchprobe
+EXCLUDE_MODULES = tools/laser tools/filamentdetector tools/scaracal tools/temperaturecontrol tools/extruder tools/zprobe tools/endstops
# e.g for a CNC machine
#export EXCLUDED_MODULES = tools/touchprobe tools/laser tools/temperaturecontrol tools/extruder
#include "Gcode.h"
#include "libs/StreamOutputPool.h"
#include "Stepper.h"
+#include "StepTicker.h"
#include "mri.h"
using std::string;
#include <vector>
+#define STEP_TICKER_FREQUENCY THEKERNEL->step_ticker->get_frequency()
+#define STEP_TICKER_FREQUENCY_2 (STEP_TICKER_FREQUENCY*STEP_TICKER_FREQUENCY)
+
// A block represents a movement, it's length for each stepper motor, and the corresponding acceleration curves.
// It's stacked on a queue, and that queue is then executed in order, to move the motors.
// Most of the accel math is also done in this class
millimeters = 0.0F;
entry_speed = 0.0F;
exit_speed = 0.0F;
- rate_delta = 0.0F;
acceleration = 100.0F; // we don't want to get devide by zeroes if this is not set
initial_rate = -1;
- final_rate = -1;
accelerate_until = 0;
decelerate_after = 0;
direction_bits = 0;
max_entry_speed = 0.0F;
is_ready = false;
times_taken = 0;
+ acceleration_per_tick= 0;
+ deceleration_per_tick= 0;
+ total_move_ticks= 0;
}
void Block::debug()
{
- THEKERNEL->streams->printf("%p: steps:X%04lu Y%04lu Z%04lu(max:%4lu) nominal:r%10lu/s%6.1f mm:%9.6f rdelta:%8f acc:%5lu dec:%5lu rates:%10lu>%10lu entry/max: %10.4f/%10.4f taken:%d ready:%d recalc:%d nomlen:%d\r\n",
+ THEKERNEL->streams->printf("%p: steps:X%04lu Y%04lu Z%04lu(max:%4lu) nominal:r%10lu/s%6.1f mm:%9.6f acc:%5lu dec:%5lu rates:%10lu entry/max: %10.4f/%10.4f taken:%d ready:%d recalc:%d nomlen:%d\r\n",
this,
this->steps[0],
this->steps[1],
this->nominal_rate,
this->nominal_speed,
this->millimeters,
- this->rate_delta,
this->accelerate_until,
this->decelerate_after,
this->initial_rate,
- this->final_rate,
this->entry_speed,
this->max_entry_speed,
this->times_taken,
if (times_taken)
return;
- // The planner passes us factors, we need to transform them in rates
- this->initial_rate = ceilf(this->nominal_rate * entryspeed / this->nominal_speed); // (step/s)
- this->final_rate = ceilf(this->nominal_rate * exitspeed / this->nominal_speed); // (step/s)
-
- // How many steps to accelerate and decelerate
- float acceleration_per_second = this->rate_delta * THEKERNEL->acceleration_ticks_per_second; // ( step/s^2)
- int accelerate_steps = ceilf( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_second ) );
- int decelerate_steps = floorf( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate, -acceleration_per_second ) );
-
- // Calculate the size of Plateau of Nominal Rate ( during which we don't accelerate nor decelerate, but just cruise )
- int plateau_steps = this->steps_event_count - accelerate_steps - decelerate_steps;
-
- // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
- // have to use intersection_distance() to calculate when to abort acceleration and start braking
- // in order to reach the final_rate exactly at the end of this block.
- if (plateau_steps < 0) {
- accelerate_steps = ceilf(this->intersection_distance(this->initial_rate, this->final_rate, acceleration_per_second, this->steps_event_count));
- accelerate_steps = max( accelerate_steps, 0 ); // Check limits due to numerical round-off
- accelerate_steps = min( accelerate_steps, int(this->steps_event_count) );
- plateau_steps = 0;
+ float initial_rate = this->nominal_rate * (entryspeed / this->nominal_speed); // steps/sec
+ float final_rate = this->nominal_rate * (exitspeed / this->nominal_speed);
+ //printf("Initial rate: %f, final_rate: %f\n", initial_rate, final_rate);
+ // How many steps ( can be fractions of steps, we need very precise values ) to accelerate and decelerate
+ // This is a simplification to get rid of rate_delta and get the steps/s² accel directly from the mm/s² accel
+ float acceleration_per_second = (this->acceleration * this->steps_event_count) / this->millimeters;
+
+ float maximum_possible_rate = sqrtf( ( this->steps_event_count * acceleration_per_second ) + ( ( powf(initial_rate, 2) + powf(final_rate, 2) ) / 2.0F ) );
+
+ //printf("id %d: acceleration_per_second: %f, maximum_possible_rate: %f steps/sec, %f mm/sec\n", this->id, acceleration_per_second, maximum_possible_rate, maximum_possible_rate/100);
+
+ // Now this is the maximum rate we'll achieve this move, either because
+ // it's the higher we can achieve, or because it's the higher we are
+ // allowed to achieve
+ this->maximum_rate = std::min(maximum_possible_rate, (float)this->nominal_rate);
+
+ // Now figure out how long it takes to accelerate in seconds
+ float time_to_accelerate = ( this->maximum_rate - initial_rate ) / acceleration_per_second;
+
+ // Now figure out how long it takes to decelerate
+ float time_to_decelerate = ( final_rate - this->maximum_rate ) / -acceleration_per_second;
+
+ // Now we know how long it takes to accelerate and decelerate, but we must
+ // also know how long the entire move takes so we can figure out how long
+ // is the plateau if there is one
+ float plateau_time = 0;
+
+ // Only if there is actually a plateau ( we are limited by nominal_rate )
+ if(maximum_possible_rate > this->nominal_rate) {
+ // Figure out the acceleration and deceleration distances ( in steps )
+ float acceleration_distance = ( ( initial_rate + this->maximum_rate ) / 2.0F ) * time_to_accelerate;
+ float deceleration_distance = ( ( this->maximum_rate + final_rate ) / 2.0F ) * time_to_decelerate;
+
+ // Figure out the plateau steps
+ float plateau_distance = this->steps_event_count - acceleration_distance - deceleration_distance;
+
+ // Figure out the plateau time in seconds
+ plateau_time = plateau_distance / this->maximum_rate;
}
- this->accelerate_until = accelerate_steps;
- this->decelerate_after = accelerate_steps + plateau_steps;
+ // Figure out how long the move takes total ( in seconds )
+ float total_move_time = time_to_accelerate + time_to_decelerate + plateau_time;
+ //puts "total move time: #{total_move_time}s time to accelerate: #{time_to_accelerate}, time to decelerate: #{time_to_decelerate}"
+
+ // We now have the full timing for acceleration, plateau and deceleration,
+ // yay \o/ Now this is very important these are in seconds, and we need to
+ // round them into ticks. This means instead of accelerating in 100.23
+ // ticks we'll accelerate in 100 ticks. Which means to reach the exact
+ // speed we want to reach, we must figure out a new/slightly different
+ // acceleration/deceleration to be sure we accelerate and decelerate at
+ // the exact rate we want
+
+ // First off round total time, acceleration time and deceleration time in ticks
+ uint32_t acceleration_ticks = floorf( time_to_accelerate * STEP_TICKER_FREQUENCY );
+ uint32_t deceleration_ticks = floorf( time_to_decelerate * STEP_TICKER_FREQUENCY );
+ uint32_t total_move_ticks = floorf( total_move_time * STEP_TICKER_FREQUENCY );
+
+ // Now deduce the plateau time for those new values expressed in tick
+ //uint32_t plateau_ticks = total_move_ticks - acceleration_ticks - deceleration_ticks;
+
+ // Now we figure out the acceleration value to reach EXACTLY maximum_rate(steps/s) in EXACTLY acceleration_ticks(ticks) amount of time in seconds
+ float acceleration_time = acceleration_ticks / STEP_TICKER_FREQUENCY; // This can be moved into the operation below, separated for clarity, note we need to do this instead of using time_to_accelerate(seconds) directly because time_to_accelerate(seconds) and acceleration_ticks(seconds) do not have the same value anymore due to the rounding
+ float deceleration_time = deceleration_ticks / STEP_TICKER_FREQUENCY;
+
+ float acceleration_in_steps = (acceleration_time > 0.0F ) ? ( this->maximum_rate - initial_rate ) / acceleration_time : 0;
+ float deceleration_in_steps = (deceleration_time > 0.0F ) ? ( this->maximum_rate - final_rate ) / deceleration_time : 0;
+
+ // Note we use this value for acceleration as well as for deceleration, if that doesn't work, we can also as well compute the deceleration value this way :
+ // float deceleration(steps/s²) = ( final_rate(steps/s) - maximum_rate(steps/s) ) / acceleration_time(s);
+ // and store that in the block and use it for deceleration, which -will- yield better results, but may not be useful. If the moves do not end correctly, try computing this value, adding it to the block, and then using it for deceleration in the step generator
+
+ // Now figure out the two acceleration ramp change events in ticks
+ this->accelerate_until = acceleration_ticks;
+ this->decelerate_after = total_move_ticks - deceleration_ticks;
+
+ // Now figure out the acceleration PER TICK, this should ideally be held as a float, even a double if possible as it's very critical to the block timing
+ // steps/tick^2
+
+ this->acceleration_per_tick = acceleration_in_steps / STEP_TICKER_FREQUENCY_2;
+ this->deceleration_per_tick = deceleration_in_steps / STEP_TICKER_FREQUENCY_2;
+
+ // We now have everything we need for this block to call a Steppermotor->move method !!!!
+ // Theorically, if accel is done per tick, the speed curve should be perfect.
+
+ // We need this to call move()
+ this->total_move_ticks = total_move_ticks;
+
+ //puts "accelerate_until: #{this->accelerate_until}, decelerate_after: #{this->decelerate_after}, acceleration_per_tick: #{this->acceleration_per_tick}, total_move_ticks: #{this->total_move_ticks}"
+
+ this->initial_rate = initial_rate;
this->exit_speed = exitspeed;
}
float millimeters; // Distance for this move
float entry_speed;
float exit_speed;
- float rate_delta; // Number of steps to add to the speed for each acceleration tick
float acceleration; // the acceleratoin for this block
uint32_t initial_rate; // Initial speed in steps per second
- uint32_t final_rate; // Final speed in steps per second
uint32_t accelerate_until; // Stop accelerating after this number of steps
uint32_t decelerate_after; // Start decelerating after this number of steps
+ float maximum_rate;
+
+ float acceleration_per_tick{0};
+ float deceleration_per_tick {0};
+ uint32_t total_move_ticks{0};
float max_entry_speed;
// average travel per step event changes. For a line along one axis the travel per step event
// is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both
// axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2).
- // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed
- // specifically for each line to compensate for this phenomenon:
- // Convert universal acceleration for direction-dependent stepper rate change parameter
- block->rate_delta = (block->steps_event_count * acceleration) / (distance * THEKERNEL->acceleration_ticks_per_second); // (step/min/acceleration_tick)
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
-/*
- This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl) with additions from Sungeun K. Jeon (https://github.com/chamnit/grbl)
- Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
- Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
- You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
-*/
-
-#include "Stepper.h"
-
-#include "libs/Module.h"
-#include "libs/Kernel.h"
-#include "Planner.h"
-#include "Conveyor.h"
-#include "StepperMotor.h"
-#include "Robot.h"
-#include "checksumm.h"
-#include "SlowTicker.h"
-#include "Config.h"
-#include "ConfigValue.h"
-#include "Gcode.h"
-#include "Block.h"
-#include "StepTicker.h"
-
-#include <vector>
-using namespace std;
-
-#include "libs/nuts_bolts.h"
-#include "libs/Hook.h"
-
-#include <mri.h>
-
-// The stepper reacts to blocks that have XYZ movement to transform them into actual stepper motor moves
-// TODO: This does accel, accel should be in StepperMotor
-
-Stepper::Stepper()
-{
- this->current_block = NULL;
- this->force_speed_update = false;
-}
-
-//Called when the module has just been loaded
-void Stepper::on_module_loaded()
-{
- this->register_for_event(ON_BLOCK_BEGIN);
- this->register_for_event(ON_BLOCK_END);
- this->register_for_event(ON_GCODE_RECEIVED);
- this->register_for_event(ON_HALT);
-
- // Get onfiguration
- this->on_config_reload(this);
-
- // Acceleration ticker
- THEKERNEL->step_ticker->register_acceleration_tick_handler([this](){trapezoid_generator_tick(); });
-
- // Attach to the end_of_move stepper event
- for (auto actuator : THEKERNEL->robot->actuators)
- actuator->attach(this, &Stepper::stepper_motor_finished_move );
-}
-
-// Get configuration from the config file
-void Stepper::on_config_reload(void *argument)
-{
- // Steppers start off by default
- this->turn_enable_pins_off();
-}
-
-void Stepper::on_halt(void *argument)
-{
- if(argument == nullptr) {
- this->turn_enable_pins_off();
- }
-}
-
-void Stepper::on_gcode_received(void *argument)
-{
- Gcode *gcode = static_cast<Gcode *>(argument);
-
- if( gcode->has_m) {
- if( gcode->m == 17 ) {
- this->turn_enable_pins_on();
-
- }else if( (gcode->m == 84 || gcode->m == 18) && !gcode->has_letter('E') ) {
- THEKERNEL->conveyor->wait_for_empty_queue();
- this->turn_enable_pins_off();
- }
- }
-}
-
-// Enable steppers
-void Stepper::turn_enable_pins_on()
-{
- for (auto a : THEKERNEL->robot->actuators)
- a->enable(true);
- this->enable_pins_status = true;
- THEKERNEL->call_event(ON_ENABLE, (void*)1);
-}
-
-// Disable steppers
-void Stepper::turn_enable_pins_off()
-{
- for (auto a : THEKERNEL->robot->actuators)
- a->enable(false);
- this->enable_pins_status = false;
- THEKERNEL->call_event(ON_ENABLE, nullptr);
-}
-
-// A new block is popped from the queue
-void Stepper::on_block_begin(void *argument)
-{
- Block *block = static_cast<Block *>(argument);
-
- // Mark the new block as of interrest to us, handle blocks that have no axis moves properly (like Extrude blocks etc)
- bool take = false;
- if (block->millimeters > 0.0F) {
- for (size_t s = 0; !take && s < THEKERNEL->robot->actuators.size(); s++) {
- take = block->steps[s] > 0;
- }
- }
- if (take){
- block->take();
- } else {
- // none of the steppers move this block so make sure they know that
- for(auto a : THEKERNEL->robot->actuators) {
- a->set_moved_last_block(false);
- }
- return;
- }
-
- // We can't move with the enable pins off
- if( this->enable_pins_status == false ) {
- this->turn_enable_pins_on();
- }
-
- // Setup : instruct stepper motors to move
- // Find the stepper with the more steps, it's the one the speed calculations will want to follow
- this->main_stepper = nullptr;
- int most_steps_to_move = 0;
- for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++) {
- if (block->steps[i] > 0) {
- THEKERNEL->robot->actuators[i]->move(block->direction_bits[i], block->steps[i])->set_moved_last_block(true);
- int steps_to_move = THEKERNEL->robot->actuators[i]->get_steps_to_move();
- if (steps_to_move > most_steps_to_move) {
- most_steps_to_move = steps_to_move;
- this->main_stepper = THEKERNEL->robot->actuators[i];
- }
- }
- else {
- THEKERNEL->robot->actuators[i]->set_moved_last_block(false);
- }
- }
-
- this->current_block = block;
-
- // Setup acceleration for this block
- this->trapezoid_generator_reset();
-
- // Set the initial speed for this move
- this->trapezoid_generator_tick();
-
- // synchronize the acceleration timer with the start of the new block so it does not drift and randomly fire during the block
- THEKERNEL->step_ticker->synchronize_acceleration(false);
-
- // set a flag to synchronize the acceleration timer with the deceleration step, and fire it immediately we get to that step
- if( block->decelerate_after > 0 && block->decelerate_after+1 < this->main_stepper->steps_to_move ) {
- this->main_stepper->signal_step= block->decelerate_after+1; // we make it +1 as deceleration does not start until steps > decelerate_after
- }
-}
-
-// Current block is discarded
-void Stepper::on_block_end(void *argument)
-{
- this->current_block = NULL; //stfu !
-}
-
-// When a stepper motor has finished it's assigned movement
-uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy)
-{
- // We care only if none is still moving
- for (auto a : THEKERNEL->robot->actuators) {
- if(a->moving)
- return 0;
- }
-
- // This block is finished, release it
- if( this->current_block != NULL ) {
- this->current_block->release();
- }
-
- return 0;
-}
-
-
-// This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
-// interrupt. It can be assumed that the trapezoid-generator-parameters and the
-// current_block stays untouched by outside handlers for the duration of this function call.
-// NOTE caled at the same priority as PendSV so it may make that longer but it is better that having htis pre empted by pendsv
-void Stepper::trapezoid_generator_tick(void)
-{
- // Do not do the accel math for nothing
- if(this->current_block && this->main_stepper->moving ) {
-
- // Store this here because we use it a lot down there
- uint32_t current_steps_completed = this->main_stepper->stepped;
- float last_rate= trapezoid_adjusted_rate;
-
- if( this->force_speed_update ) {
- // Do not accel, just set the value
- this->force_speed_update = false;
- last_rate= -1;
-
- } else if(THEKERNEL->conveyor->is_flushing()) {
- // if we are flushing the queue, decelerate to 0 then finish this block
- if (trapezoid_adjusted_rate > current_block->rate_delta * 1.5F) {
- trapezoid_adjusted_rate -= current_block->rate_delta;
-
- } else if (trapezoid_adjusted_rate == current_block->rate_delta * 0.5F) {
- for (auto i : THEKERNEL->robot->actuators) i->move(i->direction, 0); // stop motors
- if (current_block) current_block->release();
- THEKERNEL->call_event(ON_SPEED_CHANGE, 0); // tell others we stopped
- return;
-
- } else {
- trapezoid_adjusted_rate = current_block->rate_delta * 0.5F;
- }
-
- } else if(current_steps_completed <= this->current_block->accelerate_until) {
- // If we are accelerating
- // Increase speed
- this->trapezoid_adjusted_rate += this->current_block->rate_delta;
- if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
- this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
- }
-
- } else if (current_steps_completed > this->current_block->decelerate_after) {
- // If we are decelerating
- // Reduce speed
- // NOTE: We will only reduce speed if the result will be > 0. This catches small
- // rounding errors that might leave steps hanging after the last trapezoid tick.
- if(this->trapezoid_adjusted_rate > this->current_block->rate_delta * 1.5F) {
- this->trapezoid_adjusted_rate -= this->current_block->rate_delta;
- } else {
- this->trapezoid_adjusted_rate = this->current_block->rate_delta * 1.5F;
- }
- if(this->trapezoid_adjusted_rate < this->current_block->final_rate ) {
- this->trapezoid_adjusted_rate = this->current_block->final_rate;
- }
-
- } else if (trapezoid_adjusted_rate != current_block->nominal_rate) {
- // If we are cruising
- // Make sure we cruise at exactly nominal rate
- this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
- }
-
- if(last_rate != trapezoid_adjusted_rate) {
- // don't call this if speed did not change
- this->set_step_events_per_second(this->trapezoid_adjusted_rate);
- }
- }
-}
-
-// Initializes the trapezoid generator from the current block. Called whenever a new
-// block begins.
-inline void Stepper::trapezoid_generator_reset()
-{
- this->trapezoid_adjusted_rate = this->current_block->initial_rate;
- this->force_speed_update = true;
-}
-
-// Update the speed for all steppers
-void Stepper::set_step_events_per_second( float steps_per_second )
-{
- float isps= steps_per_second / this->current_block->steps_event_count;
-
- // Instruct the stepper motors
- for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++) {
- if (THEKERNEL->robot->actuators[i]->moving) {
- THEKERNEL->robot->actuators[i]->set_speed(isps * this->current_block->steps[i]);
- }
- }
-
- // Other modules might want to know the speed changed
- THEKERNEL->call_event(ON_SPEED_CHANGE, this);
-}
-
-
+/*
+ This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl) with additions from Sungeun K. Jeon (https://github.com/chamnit/grbl)
+ Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
+ Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+ You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "Stepper.h"
+
+#include "libs/Module.h"
+#include "libs/Kernel.h"
+#include "Planner.h"
+#include "Conveyor.h"
+#include "StepperMotor.h"
+#include "Robot.h"
+#include "checksumm.h"
+#include "SlowTicker.h"
+#include "Config.h"
+#include "ConfigValue.h"
+#include "Gcode.h"
+#include "Block.h"
+#include "StepTicker.h"
+
+#include <functional>
+
+#include "libs/nuts_bolts.h"
+#include "libs/Hook.h"
+
+#include <mri.h>
+
+// The stepper reacts to blocks that have XYZ movement to transform them into actual stepper motor moves
+Stepper::Stepper()
+{
+ this->current_block = NULL;
+}
+
+//Called when the module has just been loaded
+void Stepper::on_module_loaded()
+{
+ this->register_for_event(ON_BLOCK_BEGIN);
+ this->register_for_event(ON_BLOCK_END);
+ this->register_for_event(ON_GCODE_RECEIVED);
+ this->register_for_event(ON_HALT);
+
+ // Get onfiguration
+ this->on_config_reload(this);
+
+ // Attach to the end_of_move stepper event
+ THEKERNEL->step_ticker->finished_fnc= std::bind( &Stepper::stepper_motor_finished_move, this);
+}
+
+// Get configuration from the config file
+void Stepper::on_config_reload(void *argument)
+{
+ // Steppers start off by default
+ this->turn_enable_pins_off();
+}
+
+void Stepper::on_halt(void *argument)
+{
+ if(argument == nullptr) {
+ this->turn_enable_pins_off();
+ }
+}
+
+void Stepper::on_gcode_received(void *argument)
+{
+ Gcode *gcode = static_cast<Gcode *>(argument);
+
+ if( gcode->has_m) {
+ if( gcode->m == 17 ) {
+ this->turn_enable_pins_on();
+
+ }else if( (gcode->m == 84 || gcode->m == 18) && !gcode->has_letter('E') ) {
+ THEKERNEL->conveyor->wait_for_empty_queue();
+ this->turn_enable_pins_off();
+ }
+ }
+}
+
+// Enable steppers
+void Stepper::turn_enable_pins_on()
+{
+ for (auto a : THEKERNEL->robot->actuators)
+ a->enable(true);
+ this->enable_pins_status = true;
+ THEKERNEL->call_event(ON_ENABLE, (void*)1);
+}
+
+// Disable steppers
+void Stepper::turn_enable_pins_off()
+{
+ for (auto a : THEKERNEL->robot->actuators)
+ a->enable(false);
+ this->enable_pins_status = false;
+ THEKERNEL->call_event(ON_ENABLE, nullptr);
+}
+
+// A new block is popped from the queue
+void Stepper::on_block_begin(void *argument)
+{
+ Block *block = static_cast<Block *>(argument);
+
+ // Mark the new block as of interrest to us, handle blocks that have no axis moves properly (like Extrude blocks etc)
+ bool take = false;
+ if (block->millimeters > 0.0F) {
+ for (size_t s = 0; !take && s < THEKERNEL->robot->actuators.size(); s++) {
+ take = block->steps[s] > 0;
+ }
+ }
+ if(!take) return;
+
+ block->take();
+
+ // We can't move with the enable pins off
+ if( this->enable_pins_status == false ) {
+ this->turn_enable_pins_on();
+ }
+
+ this->current_block = block;
+
+ // setup stepticker to execute this block
+ // if it is running then add this to next block otherwise it needs to be started
+ // if(!THEKERNEL->step_ticker->is_next_block()) {
+
+ // }
+ THEKERNEL->step_ticker->copy_block(block);
+}
+
+// Current block is discarded
+void Stepper::on_block_end(void *argument)
+{
+ this->current_block = NULL; //stfu !
+}
+
+// When all moves in a block have finished this is called by step ticker (in the pendsv ISR)
+void Stepper::stepper_motor_finished_move()
+{
+ // This block is finished, release it
+ if( this->current_block != NULL ) {
+ this->current_block->release();
+ }
+}
void on_gcode_received(void *argument);
void on_halt(void *argument);
- void trapezoid_generator_reset();
- void set_step_events_per_second(float);
- void trapezoid_generator_tick(void);
- uint32_t stepper_motor_finished_move(uint32_t dummy);
- int config_step_timer( int cycles );
void turn_enable_pins_on();
void turn_enable_pins_off();
- float get_trapezoid_adjusted_rate() const { return trapezoid_adjusted_rate; }
const Block *get_current_block() const { return current_block; }
private:
+ void stepper_motor_finished_move();
+
Block *current_block;
- float trapezoid_adjusted_rate;
- StepperMotor *main_stepper;
struct {
bool enable_pins_status:1;
- bool force_speed_update:1;
};
};
return;
}
-// Speed has been updated for the robot's stepper, we must update accordingly
-void Extruder::on_speed_change( void *argument )
-{
- // Avoid trying to work when we really shouldn't ( between blocks or re-entry )
- if(!this->enabled || this->current_block == NULL || this->mode != FOLLOW || !this->stepper_motor->is_moving()) {
- return;
- }
-
- // if we are flushing the queue we need to stop the motor when it has decelerated to zero, we get this call with argumnet == 0 when this happens
- // this is what steppermotor does
- if(argument == 0) {
- this->stepper_motor->move(0, 0);
- this->current_block->release();
- this->current_block = NULL;
- return;
- }
-
- /*
- * nominal block duration = current block's steps / ( current block's nominal rate )
- * nominal extruder rate = extruder steps / nominal block duration
- * actual extruder rate = nominal extruder rate * ( ( stepper's steps per second ) / ( current block's nominal rate ) )
- * or actual extruder rate = ( ( extruder steps * ( current block's nominal_rate ) ) / current block's steps ) * ( ( stepper's steps per second ) / ( current block's nominal rate ) )
- * or simplified : extruder steps * ( stepper's steps per second ) ) / current block's steps
- * or even : ( stepper steps per second ) * ( extruder steps / current block's steps )
- */
-
- this->stepper_motor->set_speed(THEKERNEL->stepper->get_trapezoid_adjusted_rate() * (float)this->stepper_motor->get_steps_to_move() / (float)this->current_block->steps_event_count);
-}
-
// When the stepper has finished it's move
uint32_t Extruder::stepper_motor_finished_move(uint32_t dummy)
{
void SimpleShell::print_thermistors_command( string parameters, StreamOutput *stream)
{
+ #ifndef NO_TOOLS_TEMPERATURECONTROL
Thermistor::print_predefined_thermistors(stream);
+ #endif
}
void SimpleShell::calc_thermistor_command( string parameters, StreamOutput *stream)
{
+ #ifndef NO_TOOLS_TEMPERATURECONTROL
string s = shift_parameter( parameters );
int saveto= -1;
// see if we have -sn as first argument
// give help
stream->printf("Usage: calc_thermistor T1,R1,T2,R2,T3,R3\n");
}
+ #endif
}
// used to test out the get public data events for switch
HCoop / clinton / Smoothieware.git / commitdiff