# Stepper module configuration
microseconds_per_step_pulse 1 # Duration of step pulses to stepper drivers, in microseconds
-minimum_steps_per_minute 1200 # Never step slower than this
base_stepping_frequency 100000 # Base frequency for stepping, higher gives smoother movement
# Stepper module pins ( ports, and pin numbers, appending "!" to the number will invert a pin )
# Stepper module configuration
microseconds_per_step_pulse 1 # Duration of step pulses to stepper drivers, in microseconds
-minimum_steps_per_minute 1200 # Never step slower than this
base_stepping_frequency 100000 # Base frequency for stepping, higher gives smoother movement
# Stepper module pins ( ports, and pin numbers, appending "!" to the number will invert a pin )
# Stepper module configuration
microseconds_per_step_pulse 1 # Duration of step pulses to stepper drivers, in microseconds
-minimum_steps_per_minute 1200 # Never step slower than this
base_stepping_frequency 100000 # Base frequency for stepping, higher gives smoother movement
+#number_of_motors 5 # total number of stepper motors only change if adding extra external extruders
# Cartesian axis speed limits
x_axis_max_speed 30000 # mm/min
# Stepper module configuration
microseconds_per_step_pulse 1 # Duration of step pulses to stepper drivers, in microseconds
-minimum_steps_per_minute 1200 # Never step slower than this
base_stepping_frequency 100000 # Base frequency for stepping, higher gives smoother movement
+#number_of_motors 5 # total number of stepper motors only change if adding extra external extruders
+
# Cartesian axis speed limits
x_axis_max_speed 30000 # mm/min
y_axis_max_speed 30000 # mm/min
#define base_stepping_frequency_checksum CHECKSUM("base_stepping_frequency")
#define microseconds_per_step_pulse_checksum CHECKSUM("microseconds_per_step_pulse")
+#define number_of_motors_checksum CHECKSUM("number_of_motors")
Kernel* Kernel::instance;
// HAL stuff
add_module( this->slow_ticker = new SlowTicker());
- this->step_ticker = new StepTicker();
+
+ int nmotors= this->config->value(number_of_motors_checksum)->by_default(5)->as_number(); // default X Y Z E1 E2
+ this->step_ticker = new StepTicker(nmotors);
this->adc = new Adc();
// TODO : These should go into platform-specific files
float microseconds_per_step_pulse = this->config->value(microseconds_per_step_pulse_checksum )->by_default(5 )->as_number();
// Configure the step ticker ( TODO : shouldnt this go into stepticker's code ? )
- this->step_ticker->set_reset_delay( microseconds_per_step_pulse / 1000000L );
+ this->step_ticker->set_reset_delay( microseconds_per_step_pulse / 1000000.0F );
this->step_ticker->set_frequency( this->base_stepping_frequency );
// Core modules
Pin::Pin(){
this->inverting= false;
+ this->valid= false;
+ this->pin= 32;
+ this->port= nullptr;
}
// Make a new pin object from a string
const char* cs = value.c_str();
// cn is the position of the next char after the number we just read
char* cn = NULL;
+ valid= true;
// grab first integer as port. pointer to first non-digit goes in cn
this->port_number = strtol(cs, &cn, 10);
}
// from_string failed. TODO: some sort of error
+ valid= false;
port_number = 0;
port = gpios[0];
- pin = 255;
+ pin = 32;
inverting = false;
return this;
}
// Configure this pin as OD
Pin* Pin::as_open_drain(){
- if (this->pin >= 32) return this;
+ if (!this->valid) return this;
if( this->port_number == 0 ){ LPC_PINCON->PINMODE_OD0 |= (1<<this->pin); }
if( this->port_number == 1 ){ LPC_PINCON->PINMODE_OD1 |= (1<<this->pin); }
if( this->port_number == 2 ){ LPC_PINCON->PINMODE_OD2 |= (1<<this->pin); }
// Configure this pin as a repeater
Pin* Pin::as_repeater(){
- if (this->pin >= 32) return this;
+ if (!this->valid) return this;
// Set the two bits for this pin as 01
if( this->port_number == 0 && this->pin < 16 ){ LPC_PINCON->PINMODE0 |= (1<<( this->pin*2)); LPC_PINCON->PINMODE0 &= ~(2<<( this->pin *2)); }
if( this->port_number == 0 && this->pin >= 16 ){ LPC_PINCON->PINMODE1 |= (1<<( this->pin*2)); LPC_PINCON->PINMODE1 &= ~(2<<((this->pin-16)*2)); }
// Configure this pin as no pullup or pulldown
Pin* Pin::pull_none(){
- if (this->pin >= 32) return this;
+ if (!this->valid) return this;
// Set the two bits for this pin as 10
if( this->port_number == 0 && this->pin < 16 ){ LPC_PINCON->PINMODE0 |= (2<<( this->pin*2)); LPC_PINCON->PINMODE0 &= ~(1<<( this->pin *2)); }
if( this->port_number == 0 && this->pin >= 16 ){ LPC_PINCON->PINMODE1 |= (2<<( this->pin*2)); LPC_PINCON->PINMODE1 &= ~(1<<((this->pin-16)*2)); }
// Configure this pin as a pullup
Pin* Pin::pull_up(){
- if (this->pin >= 32) return this;
+ if (!this->valid) return this;
// Set the two bits for this pin as 00
if( this->port_number == 0 && this->pin < 16 ){ LPC_PINCON->PINMODE0 &= ~(3<<( this->pin *2)); }
if( this->port_number == 0 && this->pin >= 16 ){ LPC_PINCON->PINMODE1 &= ~(3<<((this->pin-16)*2)); }
// Configure this pin as a pulldown
Pin* Pin::pull_down(){
- if (this->pin >= 32) return this;
+ if (!this->valid) return this;
// Set the two bits for this pin as 11
if( this->port_number == 0 && this->pin < 16 ){ LPC_PINCON->PINMODE0 |= (3<<( this->pin *2)); }
if( this->port_number == 0 && this->pin >= 16 ){ LPC_PINCON->PINMODE1 |= (3<<((this->pin-16)*2)); }
Pin* from_string(std::string value);
inline bool connected(){
- return this->pin < 32;
+ return this->valid;
}
inline bool equals(const Pin& other) const {
}
inline Pin* as_output(){
- if (this->pin < 32)
+ if (this->valid)
this->port->FIODIR |= 1<<this->pin;
return this;
}
inline Pin* as_input(){
- if (this->pin < 32)
+ if (this->valid)
this->port->FIODIR &= ~(1<<this->pin);
return this;
}
Pin* pull_none(void);
inline bool get(){
-
- if (this->pin >= 32) return false;
+ if (!this->valid) return false;
return this->inverting ^ (( this->port->FIOPIN >> this->pin ) & 1);
}
inline void set(bool value)
{
- if (this->pin >= 32) return;
+ if (!this->valid) return;
if ( this->inverting ^ value )
this->port->FIOSET = 1 << this->pin;
else
}
mbed::PwmOut *hardware_pwm();
-
+
+ // these should be private, and use getters
LPC_GPIO_TypeDef* port;
- bool inverting;
- char port_number;
+
unsigned char pin;
+ char port_number;
+ struct {
+ bool inverting:1;
+ bool valid:1;
+ };
};
void SlowTicker::tick(){
// Call all hooks that need to be called ( bresenham )
- for (uint32_t i=0; i<this->hooks.size(); i++){
- Hook* hook = this->hooks.at(i);
+ for (Hook* hook : this->hooks){
hook->countdown -= this->interval;
if (hook->countdown < 0)
{
// TODO replace this with std::function()
template<typename T> Hook* attach( uint32_t frequency, T *optr, uint32_t ( T::*fptr )( uint32_t ) ){
Hook* hook = new Hook();
- hook->interval = int(floor((SystemCoreClock/4)/frequency));
+ hook->interval = floorf((SystemCoreClock/4)/frequency);
hook->attach(optr, fptr);
hook->countdown = hook->interval;
#include "StepTicker.h"
-using namespace std;
-#include <vector>
-
#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
+
extern bool _isr_context;
// StepTicker handles the base frequency ticking for the Stepper Motors / Actuators
StepTicker* StepTicker::global_step_ticker;
-StepTicker::StepTicker(){
+StepTicker::StepTicker(int nmotors){
StepTicker::global_step_ticker = this;
// Configure the timer
LPC_TIM1->TCR = 1; // Enable interrupt
// Default start values
- this->moves_finished = false;
+ this->a_move_finished = false;
this->reset_step_pins = false;
- this->debug = 0;
- this->has_axes = 0;
this->set_frequency(0.001);
this->set_reset_delay(100);
this->last_duration = 0;
- for (int i = 0; i < 12; i++){
- this->active_motors[i] = NULL;
+ //this->overruns= 0;
+ this->num_motors= nmotors;
+ this->active_motors= new StepperMotor*[num_motors];
+ for (int i = 0; i < num_motors; i++){
+ this->active_motors[i] = nullptr;
}
this->active_motor_bm = 0;
NVIC_EnableIRQ(TIMER1_IRQn); // Enable interrupt handler
}
+StepTicker::~StepTicker() {
+ delete[] this->active_motors;
+}
+
// Set the base stepping frequency
void StepTicker::set_frequency( float frequency ){
this->frequency = frequency;
- this->period = int(floor((SystemCoreClock/4)/frequency)); // SystemCoreClock/4 = Timer increments in a second
+ 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
// Set the reset delay
void StepTicker::set_reset_delay( float seconds ){
- this->delay = int(floor(float(SystemCoreClock/4)*( seconds ))); // SystemCoreClock/4 = Timer increments in a second
+ this->delay = floorf((SystemCoreClock/4.0F)*seconds); // SystemCoreClock/4 = Timer increments in a second
LPC_TIM1->MR0 = this->delay;
}
-// Add a stepper motor object to our list of steppers we must take care of
-StepperMotor* StepTicker::add_stepper_motor(StepperMotor* stepper_motor){
- this->stepper_motors.push_back(stepper_motor);
- stepper_motor->step_ticker = this;
- this->has_axes = true;
- return stepper_motor;
-}
-
// Call tick() on each active motor
inline void StepTicker::tick(){
_isr_context = true;
int i;
uint32_t bm = 1;
// We iterate over each active motor
- for (i = 0; i < 12; i++, bm <<= 1){
+ for (i = 0; i < num_motors; i++, bm <<= 1){
if (this->active_motor_bm & bm){
this->active_motors[i]->tick();
}
_isr_context = false;
}
-// Call signal_mode_finished() on each active motor that asked to be signaled. We do this instead of inside of tick() so that
+// 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_moves_finished(){
+void StepTicker::signal_a_move_finished(){
_isr_context = true;
uint16_t bitmask = 1;
- for ( uint8_t motor = 0; motor < 12; motor++, bitmask <<= 1){
+ for ( uint8_t motor = 0; motor < num_motors; motor++, bitmask <<= 1){
if (this->active_motor_bm & bitmask){
if(this->active_motors[motor]->is_move_finished){
this->active_motors[motor]->signal_move_finished();
- if(this->active_motors[motor]->moving == false){
- if (motor > 0){
- motor--;
- bitmask >>= 1;
- }
- }
+ // Theoretically this does nothing and the reason for it is currently unknown and/or forgotten
+ // if(this->active_motors[motor]->moving == false){
+ // if (motor > 0){
+ // motor--;
+ // bitmask >>= 1;
+ // }
+ // }
}
}
}
- this->moves_finished = false;
+ this->a_move_finished = false;
_isr_context = false;
}
int i;
uint32_t bm;
- for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
+ for (i = 0, bm = 1; i < num_motors; i++, bm <<= 1)
{
if (this->active_motor_bm & bm)
this->active_motors[i]->unstep();
// Step pins
uint16_t bitmask = 1;
- for (uint8_t motor = 0; motor < 12; motor++, bitmask <<= 1){
+ for (uint8_t motor = 0; motor < num_motors; motor++, bitmask <<= 1){
if (this->active_motor_bm & bitmask){
this->active_motors[motor]->tick();
}
}
// If a move finished in this tick, we have to tell the actuator to act accordingly
- if( this->moves_finished ){
+ if( this->a_move_finished ){
+
+ #ifdef STEPTICKER_DEBUG_PIN
+ stepticker_debug_pin= 1;
+ #endif
// Do not get out of here before everything is nice and tidy
LPC_TIM0->MR0 = 20000000;
- this->signal_moves_finished();
+ this->signal_a_move_finished();
// If we went over the duration an interrupt is supposed to last, we have a problem
- // That can happen tipically when we change blocks, where more than usual computation is done
+ // That can happen typically when we change blocks, where more than usual computation is done
// This can be OK, if we take notice of it, which we do now
- if( LPC_TIM0->TC > this->period ){ // TODO: remove the size condition
-
+ if(LPC_TIM0->TC > this->period ){ // TODO: remove the size condition
+ //overruns++;
uint32_t start_tc = LPC_TIM0->TC;
// How many ticks we want to skip ( this does not include the current tick, but we add the time we spent doing this computation last time )
uint32_t ticks_to_skip = ( ( LPC_TIM0->TC + this->last_duration ) / this->period );
- // Next step is now to reduce this to how many steps we can *actually* skip
+ // Next step is now to reduce this to how many steps we can *actually* skip, as we do not want to cause an actual step
uint32_t ticks_we_actually_can_skip = ticks_to_skip;
int i;
uint32_t bm;
- for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
+ for (i = 0, bm = 1; i < num_motors; i++, bm <<= 1)
{
- if (this->active_motor_bm & bm)
- ticks_we_actually_can_skip =
- min(ticks_we_actually_can_skip,
- (uint32_t)((uint64_t)( (uint64_t)this->active_motors[i]->fx_ticks_per_step - (uint64_t)this->active_motors[i]->fx_counter ) >> 32)
- );
+ if((this->active_motor_bm & bm) != 0) {
+ if(this->active_motors[i]->fx_ticks_per_step > this->active_motors[i]->fx_counter) {
+ ticks_we_actually_can_skip =
+ min(ticks_we_actually_can_skip, (uint32_t)((active_motors[i]->fx_ticks_per_step - active_motors[i]->fx_counter) >> active_motors[i]->fx_shift));
+ }else{
+ ticks_we_actually_can_skip= 0;
+ }
+ }
}
// Adding to MR0 for this time is not enough, we must also increment the counters ourself artificially
- for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
+ for (i = 0, bm = 1; i < num_motors; i++, bm <<= 1)
{
if (this->active_motor_bm & bm)
- this->active_motors[i]->fx_counter += (uint64_t)((uint64_t)(ticks_we_actually_can_skip)<<32);
+ this->active_motors[i]->fx_counter += ((uint64_t)ticks_we_actually_can_skip << active_motors[i]->fx_shift);
}
// When must we have our next MR0 ? ( +1 is here to account that we are actually doing a legit MR0 match here too, not only overtime )
int difference = (int)(LPC_TIM0->TC) - (int)(start_tc);
if( difference > 0 ){ this->last_duration = (uint32_t)difference; }
+
}else{
LPC_TIM0->MR0 = this->period;
}
LPC_TIM0->MR0 += this->period;
}
+ #ifdef STEPTICKER_DEBUG_PIN
+ stepticker_debug_pin= 0;
+ #endif
}
}
{
uint32_t bm;
int i;
- for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
+ for (i = 0, bm = 1; i < num_motors; i++, bm <<= 1)
{
if (this->active_motors[i] == motor)
{
}
return;
}
- if (this->active_motors[i] == NULL)
+ if (this->active_motors[i] == nullptr)
{
this->active_motors[i] = motor;
this->active_motor_bm |= bm;
void StepTicker::remove_motor_from_active_list(StepperMotor* motor)
{
uint32_t bm; int i;
- for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
+ for (i = 0, bm = 1; i < num_motors; i++, bm <<= 1)
{
if (this->active_motors[i] == motor)
{
#ifndef STEPTICKER_H
#define STEPTICKER_H
-using namespace std;
-#include <vector>
#include <stdint.h>
class StepperMotor;
friend class StepperMotor;
static StepTicker* global_step_ticker;
- StepTicker();
+ StepTicker(int nmotors);
+ ~StepTicker();
void set_frequency( float frequency );
void tick();
- void signal_moves_finished();
- StepperMotor* add_stepper_motor(StepperMotor* stepper_motor);
+ void signal_a_move_finished();
void set_reset_delay( float seconds );
void reset_tick();
void add_motor_to_active_list(StepperMotor* motor);
void remove_motor_from_active_list(StepperMotor* motor);
void TIMER0_IRQHandler (void);
+ //uint32_t overruns;
private:
float frequency;
- vector<StepperMotor*> stepper_motors;
uint32_t delay;
uint32_t period;
- uint32_t debug;
uint32_t last_duration;
- bool has_axes;
- bool moves_finished;
- bool reset_step_pins;
-
- StepperMotor* active_motors[12];
- uint32_t active_motor_bm;
+ StepperMotor** active_motors;
+ uint32_t active_motor_bm; // limit to 32 motors
+ struct {
+ uint8_t num_motors:6; // increase for more than 32 motors
+ bool a_move_finished:1;
+ bool reset_step_pins:1;
+ };
};
#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
// TODO : Abstract this into Actuator
set_high_on_debug(en.port_number, en.pin);
}
+StepperMotor::~StepperMotor()
+{
+ delete step_signal_hook;
+}
+
void StepperMotor::init()
{
this->moving = false;
this->stepped = 0;
this->fx_ticks_per_step = 0;
this->steps_to_move = 0;
- this->remove_from_active_list_next_reset = false;
this->is_move_finished = false;
this->signal_step = false;
this->step_signal_hook = new Hook();
steps_per_mm = 1.0F;
max_rate = 50.0F;
+ minimum_step_rate = default_minimum_actuator_rate;
- current_position_steps= 0;
last_milestone_steps = 0;
last_milestone_mm = 0.0F;
+ current_position_steps= 0;
}
{
// output to pins 37t
this->step_pin.set( 1 );
- this->step_ticker->reset_step_pins = true;
+ THEKERNEL->step_ticker->reset_step_pins = true;
// move counter back 11t
- this->fx_counter -= this->fx_ticks_per_step;
+ if(this->fx_counter > this->fx_ticks_per_step) {
+ this->fx_counter -= this->fx_ticks_per_step;
+ }else{
+ this->fx_counter= 0; // can't make it less than 0 as it is a uint, unless we get interrupted I don't think this case is possible
+ }
// we have moved a step 9t
this->stepped++;
// Mark it as finished, then StepTicker will call signal_mode_finished()
// This is so we don't call that before all the steps have been generated for this tick()
this->is_move_finished = true;
- this->step_ticker->moves_finished = true;
+ THEKERNEL->step_ticker->a_move_finished = true;
}
}
// 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 41t 411t
this->end_hook->call();
{
if( !this->moving || this->paused || this->steps_to_move == 0 ) {
// We must exit tick() after setting the pins, no bresenham is done
- //this->remove_from_active_list_next_reset = true;
- this->step_ticker->remove_motor_from_active_list(this);
+ THEKERNEL->step_ticker->remove_motor_from_active_list(this);
} else {
// We must do the bresenham in tick()
// We have to do this or there could be a bug where the removal still happens when it doesn't need to
- this->step_ticker->add_motor_to_active_list(this);
+ THEKERNEL->step_ticker->add_motor_to_active_list(this);
}
}
-
-
// Instruct the StepperMotor to move a certain number of steps
-void StepperMotor::move( bool direction, unsigned int steps )
+void StepperMotor::move( bool direction, unsigned int steps, float initial_speed)
{
- // We do not set the direction directly, we will set the pin just before the step pin on the next tick
+ // INCORRECT: We do not set the direction directly, we will set the pin just before the step pin on the next tick
+ // FIXME really? looks a lot like the pin is set here not in the next step
this->dir_pin.set(direction);
this->direction = direction;
// 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();
+}
+// this is called to set the step rate based on this blocks rate, we use this instead of set_speed for coordinated moves
+// so that we can floor to a minimum speed which is proportional for all axis. the minimum step rate is set at the start
+// of each block based on the slowest axis of all coordinated axis.
+// the rate passed in is the requested rate, it is scaled for this motor based on steps_to_move and block_steps_event_count
+void StepperMotor::set_step_rate(float requested_rate, uint32_t block_steps_event_count)
+{
+ float rate= requested_rate * ((float)steps_to_move / (float)block_steps_event_count);
+ if(rate < minimum_step_rate) {
+ rate= minimum_step_rate;
+ }
+ set_speed(rate);
}
-// Set the speed at which this steper moves
+// 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 64bit 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 32.32
void StepperMotor::set_speed( float speed )
{
-
- // FIXME NOTE this can cause axis to run faster than expected thus making the line incorrect, or on a delta make the effector move wrong
- // seems we can do... minimum_speed = ceil(step_ticker->frequency/65536.0F), which would be 2 not 20 at 100Khz
- if (speed < 20.0F)
- speed = 20.0F;
+ 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= 0xFFFFF00000000000ULL; // that is 4,294,963,200 10us ticks which is ~11.9 hours for 1 step
+ this->steps_per_second = THEKERNEL->step_ticker->frequency / (this->fx_ticks_per_step>>fx_shift);
+ return;
+ }
// How many steps we must output per second
this->steps_per_second = speed;
- // How many ticks ( base steps ) between each actual step at this speed, in fixed point 64 <--- REALLY? I don't think it is at the moment looks like 32bit fixed point
- float ticks_per_step = (float)( (float)this->step_ticker->frequency / speed );
- //float double_fx_ticks_per_step = (float)(1<<8) * ( (float)(1<<8) * ticks_per_step ); // 8x8 because we had to do 16x16 because 32 did not work
- float double_fx_ticks_per_step = 65536.0F * ticks_per_step; // isn't this better on a 32bit machine?
- this->fx_ticks_per_step = (uint32_t)( floor(double_fx_ticks_per_step) );
+ // How many ticks ( base steps ) between each actual step at this speed, in fixed point 64
+ // we need to use double here to match the 64bit resolution of the ticker
+ double ticks_per_step = (double)THEKERNEL->step_ticker->frequency / speed;
+ if(ticks_per_step > 0xFFFFF000UL) { // maximum we can really do and allow a few overflow steps
+ ticks_per_step= 0xFFFFF000UL;
+ this->steps_per_second = THEKERNEL->step_ticker->frequency / ticks_per_step;
+ }
+ double double_fx_ticks_per_step = fx_increment * ticks_per_step;
+
+ // set the new speed
+ this->fx_ticks_per_step = floor(double_fx_ticks_per_step);
}
// Pause this stepper motor
public:
StepperMotor();
StepperMotor(Pin& step, Pin& dir, Pin& en);
-
+ ~StepperMotor();
void step();
inline void unstep() { step_pin.set(0); };
bool is_moving() { return moving; }
void move_finished();
- void move( bool direction, unsigned int steps );
+ void move( bool direction, unsigned int steps, float initial_speed= -1.0F);
void signal_move_finished();
void set_speed( float speed );
+ void set_step_rate(float requested_rate, uint32_t block_steps_event_count);
+
void update_exit_tick();
void pause();
void unpause();
float get_steps_per_second() const { return steps_per_second; }
- void set_steps_per_second(float ss) { steps_per_second= ss; }
float get_steps_per_mm() const { return steps_per_mm; }
void change_steps_per_mm(float);
void change_last_milestone(float);
float get_last_milestone(void) const { return last_milestone_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; }
private:
void init();
+
Hook* end_hook;
Hook* step_signal_hook;
uint32_t signal_step_number;
- StepTicker* step_ticker;
Pin step_pin;
Pin dir_pin;
Pin en_pin;
float steps_per_second;
float steps_per_mm;
- float max_rate;
+ float max_rate; // this is not really rate it is in mm/sec, misnamed used in Robot and Extruder
+ float minimum_step_rate; // this is the minimum step_rate in steps/sec for this motor for this block
+ static float default_minimum_actuator_rate;
volatile int32_t current_position_steps;
int32_t last_milestone_steps;
uint32_t steps_to_move;
uint32_t stepped;
- uint32_t fx_counter;
- uint32_t fx_ticks_per_step;
+
+ // set to 64 bit fixed point, 32:32 bits fractional
+ static const uint32_t fx_shift= 32;
+ static const uint64_t fx_increment= ((uint64_t)1<<fx_shift);
+ uint64_t fx_counter;
+ uint64_t fx_ticks_per_step;
struct {
bool direction:1;
- bool remove_from_active_list_next_reset:1;
bool is_move_finished:1; // Whether the move just finished
bool signal_step:1;
bool paused:1;
// Called a great many times per second, to step if we have to now
inline void tick() {
- // increase the ( fixed point ) counter by one tick 11t
- fx_counter += (uint32_t)(1<<16);
+ // increase the ( 64 fixed point 32:32 ) counter by one tick 11t
+ fx_counter += fx_increment;
// if we are to step now 10t
if (fx_counter >= fx_ticks_per_step)
GPIO(P4_28)
};
+// debug pins, only used if defined in src/makefile
+#ifdef BLOCK_DEBUG_PIN
+GPIO block_debug_pin(BLOCK_DEBUG_PIN);
+#endif
+
+#ifdef STEPTICKER_DEBUG_PIN
+GPIO stepticker_debug_pin(STEPTICKER_DEBUG_PIN);
+#endif
+
void init() {
// Default pins to low status
leds[i]= 0;
}
+#ifdef BLOCK_DEBUG_PIN
+ block_debug_pin.output();
+ block_debug_pin= 0;
+#endif
+#ifdef STEPTICKER_DEBUG_PIN
+ stepticker_debug_pin.output();
+ stepticker_debug_pin= 0;
+#endif
+
Kernel* kernel = new Kernel();
kernel->streams->printf("Smoothie Running @%ldMHz\r\n", SystemCoreClock / 1000000);
# use c++11 features for the checksums and set default baud rate for serial uart
DEFINES += -DCHECKSUM_USE_CPP -DDEFAULT_SERIAL_BAUD_RATE=$(DEFAULT_SERIAL_BAUD_RATE)
+# Set a Pin here that toggles high on block begin and low on block end for debugging with logic analyzer
+#DEFINES += -DBLOCK_DEBUG_PIN=P4_29
+
+# Set a Pin here that toggles on Stepticker overrun for debugging with logic analyzer
+DEFINES += -DSTEPTICKER_DEBUG_PIN=P4_29
+
# add any modules that you do not want included in the build
export EXCLUDED_MODULES = tools/touchprobe
# e.g for a CNC machine
using std::string;
#include <vector>
+#ifdef BLOCK_DEBUG_PIN
+#include "gpio.h"
+extern GPIO block_debug_pin;
+#endif
+
// 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
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;
return;
// The planner passes us factors, we need to transform them in rates
- this->initial_rate = ceil(this->nominal_rate * entryspeed / this->nominal_speed); // (step/s)
- this->final_rate = ceil(this->nominal_rate * exitspeed / this->nominal_speed); // (step/s)
+ 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->stepper->get_acceleration_ticks_per_second(); // ( step/s^2)
- int accelerate_steps = ceil( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_second ) );
- int decelerate_steps = floor( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate, -acceleration_per_second ) );
+ 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;
// 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 = ceil(this->intersection_distance(this->initial_rate, this->final_rate, acceleration_per_second, this->steps_event_count));
+ 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;
// for max allowable speed if block is decelerating and nominal length is false.
if ((!this->nominal_length_flag) && (this->max_entry_speed > exit_speed))
{
- float max_entry_speed = max_allowable_speed(-THEKERNEL->planner->get_acceleration(), exit_speed, this->millimeters);
+ float max_entry_speed = max_allowable_speed(-this->acceleration, exit_speed, this->millimeters);
this->entry_speed = min(max_entry_speed, this->max_entry_speed);
return nominal_speed;
// otherwise, we have to work out max exit speed based on entry and acceleration
- float max = max_allowable_speed(-THEKERNEL->planner->get_acceleration(), this->entry_speed, this->millimeters);
+ float max = max_allowable_speed(-this->acceleration, this->entry_speed, this->millimeters);
return min(max, nominal_speed);
}
void Block::begin()
{
+#ifdef BLOCK_DEBUG_PIN
+ block_debug_pin= 1;
+#endif
+
recalculate_flag = false;
if (!is_ready)
for(unsigned int index = 0; index < gcodes.size(); index++)
THEKERNEL->call_event(ON_GCODE_EXECUTE, &(gcodes[index]));
+
THEKERNEL->call_event(ON_BLOCK_BEGIN, this);
if (times_taken < 0)
times_taken = 0;
if (is_ready)
{
+#ifdef BLOCK_DEBUG_PIN
+ block_debug_pin= 0;
+#endif
is_ready = false;
THEKERNEL->call_event(ON_BLOCK_END, this);
// ensure conveyor gets called last
THEKERNEL->conveyor->on_block_end(this);
+
}
}
}
void calculate_trapezoid( float entry_speed, float exit_speed );
float estimate_acceleration_distance( float initial_rate, float target_rate, float acceleration );
float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance);
- float get_duration_left(unsigned int already_taken_steps);
float max_allowable_speed( float acceleration, float target_velocity, float distance);
float reverse_pass(float exit_speed);
float entry_speed;
float exit_speed;
float rate_delta; // Nomber of steps to add to the speed for each acceleration tick
+ float acceleration; // the acceleratoin for this block
unsigned int initial_rate; // Initial speed in steps per second
unsigned int final_rate; // Final speed in steps per second
unsigned int accelerate_until; // Stop accelerating after this number of steps
unsigned int decelerate_after; // Start decelerating after this number of steps
- std::bitset<3> direction_bits; // Direction for each axis in bit form, relative to the direction port's mask
+ float max_entry_speed;
+
+ short times_taken; // A block can be "taken" by any number of modules, and the next block is not moved to until all the modules have "released" it. This value serves as a tracker.
+
+ std::bitset<3> direction_bits; // Direction for each axis in bit form, relative to the direction port's mask
struct {
bool recalculate_flag:1; // Planner flag to recalculate trapezoids on entry junction
bool nominal_length_flag:1; // Planner flag for nominal speed always reached
bool is_ready:1;
};
-
- float max_entry_speed;
-
- short times_taken; // A block can be "taken" by any number of modules, and the next block is not moved to until all the modules have "released" it. This value serves as a tracker.
-
};
if(this->z_junction_deviation >= 0.0F) junction_deviation= this->z_junction_deviation;
}
+ block->acceleration= acceleration; // save in block
+
// Max number of steps, for all axes
block->steps_event_count = max( block->steps[ALPHA_STEPPER], max( block->steps[BETA_STEPPER], block->steps[GAMMA_STEPPER] ) );
// NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c
if( distance > 0.0F ){
block->nominal_speed = rate_mm_s; // (mm/s) Always > 0
- block->nominal_rate = ceil(block->steps_event_count * rate_mm_s / distance); // (step/s) Always > 0
+ block->nominal_rate = ceilf(block->steps_event_count * rate_mm_s / distance); // (step/s) Always > 0
}else{
block->nominal_speed = 0.0F;
block->nominal_rate = 0;
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined minimum_planner_speed.
- float v_allowable = max_allowable_speed(-acceleration, minimum_planner_speed, block->millimeters); //TODO: Get from config
+ float v_allowable = max_allowable_speed(-acceleration, minimum_planner_speed, block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// The Robot converts GCodes into actual movements, and then adds them to the Planner, which passes them to the Conveyor so they can be added to the queue
// It takes care of cutting arcs into segments, same thing for line that are too long
-#define max(a,b) (((a) > (b)) ? (a) : (b))
Robot::Robot()
{
// TODO: delete or detect old steppermotors
// Make our 3 StepperMotors
- this->alpha_stepper_motor = THEKERNEL->step_ticker->add_stepper_motor( new StepperMotor(alpha_step_pin, alpha_dir_pin, alpha_en_pin) );
- this->beta_stepper_motor = THEKERNEL->step_ticker->add_stepper_motor( new StepperMotor(beta_step_pin, beta_dir_pin, beta_en_pin ) );
- this->gamma_stepper_motor = THEKERNEL->step_ticker->add_stepper_motor( new StepperMotor(gamma_step_pin, gamma_dir_pin, gamma_en_pin) );
+ this->alpha_stepper_motor = new StepperMotor(alpha_step_pin, alpha_dir_pin, alpha_en_pin);
+ this->beta_stepper_motor = new StepperMotor(beta_step_pin, beta_dir_pin, beta_en_pin );
+ this->gamma_stepper_motor = new StepperMotor(gamma_step_pin, gamma_dir_pin, gamma_en_pin);
alpha_stepper_motor->change_steps_per_mm(steps_per_mm[0]);
beta_stepper_motor->change_steps_per_mm(steps_per_mm[1]);
gamma_stepper_motor->change_steps_per_mm(steps_per_mm[2]);
- alpha_stepper_motor->max_rate = THEKERNEL->config->value(alpha_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F;
- beta_stepper_motor->max_rate = THEKERNEL->config->value(beta_max_rate_checksum )->by_default(30000.0F)->as_number() / 60.0F;
- gamma_stepper_motor->max_rate = THEKERNEL->config->value(gamma_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F;
+ alpha_stepper_motor->set_max_rate(THEKERNEL->config->value(alpha_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F);
+ beta_stepper_motor->set_max_rate(THEKERNEL->config->value(beta_max_rate_checksum )->by_default(30000.0F)->as_number() / 60.0F);
+ gamma_stepper_motor->set_max_rate(THEKERNEL->config->value(gamma_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F);
check_max_actuator_speeds(); // check the configs are sane
actuators.clear();
// we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
void Robot::check_max_actuator_speeds()
{
- float step_freq= alpha_stepper_motor->max_rate * alpha_stepper_motor->get_steps_per_mm();
+ float step_freq= alpha_stepper_motor->get_max_rate() * alpha_stepper_motor->get_steps_per_mm();
if(step_freq > THEKERNEL->base_stepping_frequency) {
- alpha_stepper_motor->max_rate= floorf(THEKERNEL->base_stepping_frequency / alpha_stepper_motor->get_steps_per_mm());
+ alpha_stepper_motor->set_max_rate(floorf(THEKERNEL->base_stepping_frequency / alpha_stepper_motor->get_steps_per_mm()));
THEKERNEL->streams->printf("WARNING: alpha_max_rate exceeds base_stepping_frequency * alpha_steps_per_mm: %f, setting to %f\n", step_freq, alpha_stepper_motor->max_rate);
}
- step_freq= beta_stepper_motor->max_rate * beta_stepper_motor->get_steps_per_mm();
+ step_freq= beta_stepper_motor->get_max_rate() * beta_stepper_motor->get_steps_per_mm();
if(step_freq > THEKERNEL->base_stepping_frequency) {
- beta_stepper_motor->max_rate= floorf(THEKERNEL->base_stepping_frequency / beta_stepper_motor->get_steps_per_mm());
+ beta_stepper_motor->set_max_rate(floorf(THEKERNEL->base_stepping_frequency / beta_stepper_motor->get_steps_per_mm()));
THEKERNEL->streams->printf("WARNING: beta_max_rate exceeds base_stepping_frequency * beta_steps_per_mm: %f, setting to %f\n", step_freq, beta_stepper_motor->max_rate);
}
- step_freq= gamma_stepper_motor->max_rate * gamma_stepper_motor->get_steps_per_mm();
+ step_freq= gamma_stepper_motor->get_max_rate() * gamma_stepper_motor->get_steps_per_mm();
if(step_freq > THEKERNEL->base_stepping_frequency) {
- gamma_stepper_motor->max_rate= floorf(THEKERNEL->base_stepping_frequency / gamma_stepper_motor->get_steps_per_mm());
+ gamma_stepper_motor->set_max_rate(floorf(THEKERNEL->base_stepping_frequency / gamma_stepper_motor->get_steps_per_mm()));
THEKERNEL->streams->printf("WARNING: gamma_max_rate exceeds base_stepping_frequency * gamma_steps_per_mm: %f, setting to %f\n", step_freq, gamma_stepper_motor->max_rate);
}
}
if (gcode->has_letter('Z'))
this->max_speeds[Z_AXIS] = gcode->get_value('Z');
if (gcode->has_letter('A'))
- alpha_stepper_motor->max_rate = gcode->get_value('A');
+ alpha_stepper_motor->set_max_rate(gcode->get_value('A'));
if (gcode->has_letter('B'))
- beta_stepper_motor->max_rate = gcode->get_value('B');
+ beta_stepper_motor->set_max_rate(gcode->get_value('B'));
if (gcode->has_letter('C'))
- gamma_stepper_motor->max_rate = gcode->get_value('C');
+ gamma_stepper_motor->set_max_rate(gcode->get_value('C'));
check_max_actuator_speeds();
gcode->stream->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ",
this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS],
- alpha_stepper_motor->max_rate, beta_stepper_motor->max_rate, gamma_stepper_motor->max_rate);
+ alpha_stepper_motor->get_max_rate(), beta_stepper_motor->get_max_rate(), gamma_stepper_motor->get_max_rate());
gcode->add_nl = true;
gcode->mark_as_taken();
break;
gcode->mark_as_taken();
if (gcode->has_letter('S')) {
- // TODO for safety so it applies only to following gcodes, maybe a better way to do this?
- THEKERNEL->conveyor->wait_for_empty_queue();
float acc = gcode->get_value('S'); // mm/s^2
// enforce minimum
if (acc < 1.0F)
THEKERNEL->planner->acceleration = acc;
}
if (gcode->has_letter('Z')) {
- // TODO for safety so it applies only to following gcodes, maybe a better way to do this?
- THEKERNEL->conveyor->wait_for_empty_queue();
float acc = gcode->get_value('Z'); // mm/s^2
// enforce positive
if (acc < 0.0F)
}
break;
- case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed
+ case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed, Ynnn - set minimum step rate
gcode->mark_as_taken();
if (gcode->has_letter('X')) {
float jd = gcode->get_value('X');
mps = 0.0F;
THEKERNEL->planner->minimum_planner_speed = mps;
}
+ if (gcode->has_letter('Y')) {
+ alpha_stepper_motor->default_minimum_actuator_rate = gcode->get_value('Y');
+ }
break;
case 220: // M220 - speed override percentage
case 503: { // M503 just prints the settings
gcode->stream->printf(";Steps per unit:\nM92 X%1.5f Y%1.5f Z%1.5f\n", actuators[0]->steps_per_mm, actuators[1]->steps_per_mm, actuators[2]->steps_per_mm);
gcode->stream->printf(";Acceleration mm/sec^2:\nM204 S%1.5f Z%1.5f\n", THEKERNEL->planner->acceleration, THEKERNEL->planner->z_acceleration);
- gcode->stream->printf(";X- Junction Deviation, Z- Z junction deviation, S - Minimum Planner speed:\nM205 X%1.5f Z%1.5f S%1.5f\n", THEKERNEL->planner->junction_deviation, THEKERNEL->planner->z_junction_deviation, THEKERNEL->planner->minimum_planner_speed);
+ gcode->stream->printf(";X- Junction Deviation, Z- Z junction deviation, S - Minimum Planner speed mm/sec:\nM205 X%1.5f Z%1.5f S%1.5f\n", THEKERNEL->planner->junction_deviation, THEKERNEL->planner->z_junction_deviation, THEKERNEL->planner->minimum_planner_speed);
gcode->stream->printf(";Max feedrates in mm/sec, XYZ cartesian, ABC actuator:\nM203 X%1.5f Y%1.5f Z%1.5f A%1.5f B%1.5f C%1.5f\n",
this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS],
- alpha_stepper_motor->max_rate, beta_stepper_motor->max_rate, gamma_stepper_motor->max_rate);
+ alpha_stepper_motor->get_max_rate(), beta_stepper_motor->get_max_rate(), gamma_stepper_motor->get_max_rate());
// get or save any arm solution specific optional values
BaseSolution::arm_options_t options;
for (int actuator = 0; actuator <= 2; actuator++) {
float actuator_rate = fabs(actuator_pos[actuator] - actuators[actuator]->last_milestone_mm) * rate_mm_s / millimeters_of_travel;
- if (actuator_rate > actuators[actuator]->max_rate)
- rate_mm_s *= (actuators[actuator]->max_rate / actuator_rate);
+ if (actuator_rate > actuators[actuator]->get_max_rate())
+ rate_mm_s *= (actuators[actuator]->get_max_rate() / actuator_rate);
}
// Append the block to the planner
// the faster the travel speed the fewer segments needed
// NOTE rate is mm/sec and we take into account any speed override
float seconds = gcode->millimeters_of_travel / rate_mm_s;
- segments = max(1, ceil(this->delta_segments_per_second * seconds));
+ segments = max(1.0F, ceilf(this->delta_segments_per_second * seconds));
// TODO if we are only moving in Z on a delta we don't really need to segment at all
} else {
if(this->mm_per_line_segment == 0.0F) {
segments = 1; // don't split it up
} else {
- segments = ceil( gcode->millimeters_of_travel / this->mm_per_line_segment);
+ segments = ceilf( gcode->millimeters_of_travel / this->mm_per_line_segment);
}
}
this->distance_in_gcode_is_known( gcode );
// Figure out how many segments for this gcode
- uint16_t segments = floor(gcode->millimeters_of_travel / this->mm_per_arc_segment);
+ uint16_t segments = floorf(gcode->millimeters_of_travel / this->mm_per_arc_segment);
float theta_per_segment = angular_travel / segments;
float linear_per_segment = linear_travel / segments;
{
this->acceleration_ticks_per_second = THEKERNEL->config->value(acceleration_ticks_per_second_checksum)->by_default(100 )->as_number();
- this->minimum_steps_per_second = THEKERNEL->config->value(minimum_steps_per_minute_checksum )->by_default(3000 )->as_number() / 60.0F;
// Steppers start off by default
this->turn_enable_pins_off();
// Attach gcodes to the last block for on_gcode_execute
if( gcode->has_m && (gcode->m == 84 || gcode->m == 17 || gcode->m == 18 )) {
THEKERNEL->conveyor->append_gcode(gcode);
-
}
}
// Setup : instruct stepper motors to move
if( block->steps[ALPHA_STEPPER] > 0 ) {
- THEKERNEL->robot->alpha_stepper_motor->move( block->direction_bits[ALPHA_STEPPER], block->steps[ALPHA_STEPPER] );
+ THEKERNEL->robot->alpha_stepper_motor->move( block->direction_bits[ALPHA_STEPPER], block->steps[ALPHA_STEPPER]);
}
if( block->steps[BETA_STEPPER ] > 0 ) {
- THEKERNEL->robot->beta_stepper_motor->move( block->direction_bits[BETA_STEPPER], block->steps[BETA_STEPPER ] );
+ THEKERNEL->robot->beta_stepper_motor->move( block->direction_bits[BETA_STEPPER], block->steps[BETA_STEPPER ]);
}
if( block->steps[GAMMA_STEPPER] > 0 ) {
- THEKERNEL->robot->gamma_stepper_motor->move( block->direction_bits[GAMMA_STEPPER], block->steps[GAMMA_STEPPER] );
+ THEKERNEL->robot->gamma_stepper_motor->move( block->direction_bits[GAMMA_STEPPER], block->steps[GAMMA_STEPPER]);
}
this->current_block = block;
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);
- if (current_block)
- current_block->release();
+ 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 0;
+
} else {
trapezoid_adjusted_rate = current_block->rate_delta * 0.5F;
}
- } else if(current_steps_completed <= this->current_block->accelerate_until + 1) {
+ } 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;
{
this->trapezoid_adjusted_rate = this->current_block->initial_rate;
this->force_speed_update = true;
- this->trapezoid_tick_cycle_counter = 0;
}
// Update the speed for all steppers
void Stepper::set_step_events_per_second( float steps_per_second )
{
- // We do not step slower than this, FIXME shoul dbe calculated for the slowest axis not the fastest
- //steps_per_second = max(steps_per_second, this->minimum_steps_per_second);
- if( steps_per_second < this->minimum_steps_per_second ) {
- steps_per_second = this->minimum_steps_per_second;
- }
-
// Instruct the stepper motors
if( THEKERNEL->robot->alpha_stepper_motor->moving ) {
- THEKERNEL->robot->alpha_stepper_motor->set_speed( steps_per_second * ( (float)this->current_block->steps[ALPHA_STEPPER] / (float)this->current_block->steps_event_count ) );
+ THEKERNEL->robot->alpha_stepper_motor->set_step_rate(steps_per_second, this->current_block->steps_event_count);
}
if( THEKERNEL->robot->beta_stepper_motor->moving ) {
- THEKERNEL->robot->beta_stepper_motor->set_speed( steps_per_second * ( (float)this->current_block->steps[BETA_STEPPER ] / (float)this->current_block->steps_event_count ) );
+ THEKERNEL->robot->beta_stepper_motor->set_step_rate(steps_per_second, this->current_block->steps_event_count);
}
if( THEKERNEL->robot->gamma_stepper_motor->moving ) {
- THEKERNEL->robot->gamma_stepper_motor->set_speed( steps_per_second * ( (float)this->current_block->steps[GAMMA_STEPPER] / (float)this->current_block->steps_event_count ) );
+ THEKERNEL->robot->gamma_stepper_motor->set_step_rate(steps_per_second , this->current_block->steps_event_count);
}
// Other modules might want to know the speed changed
THEKERNEL->call_event(ON_SPEED_CHANGE, this);
-
}
// This function has the role of making sure acceleration and deceleration curves have their
this->main_stepper->attach_signal_step(this->current_block->decelerate_after, this, &Stepper::synchronize_acceleration);
}
} else {
- // If we are called not at the first steps, this means we are beginning deceleration
+ // If we are called not at the first steps, this means we are beginning deceleratingration
NVIC_SetPendingIRQ(TIMER2_IRQn);
// Synchronize both counters
LPC_TIM2->TC = LPC_TIM0->TC;
uint32_t synchronize_acceleration(uint32_t dummy);
int get_acceleration_ticks_per_second() const { return acceleration_ticks_per_second; }
- unsigned int get_minimum_steps_per_second() const { return minimum_steps_per_second; }
float get_trapezoid_adjusted_rate() const { return trapezoid_adjusted_rate; }
const Block *get_current_block() const { return current_block; }
private:
Block *current_block;
- int counters[3];
int stepped[3];
- int offsets[3];
- float counter_alpha;
- float counter_beta;
- float counter_gamma;
- unsigned int out_bits;
float trapezoid_adjusted_rate;
- int trapezoid_tick_cycle_counter;
- int cycles_per_step_event;
- int microseconds_per_step_pulse;
int acceleration_ticks_per_second;
- unsigned int minimum_steps_per_second;
- int base_stepping_frequency;
- unsigned short step_bits[3];
- int counter_increment;
Hook *acceleration_tick_hook;
StepperMotor *main_stepper;
#define STEPPER THEKERNEL->robot->actuators
#define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm())
-#define max(a,b) (((a) > (b)) ? (a) : (b))
-#define min(a,b) (((a) <= (b)) ? (a) : (b))
// Homing States
enum{
for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
if ( ( axes_to_move >> c) & 1 ) {
this->feed_rate[c]= this->fast_rates[c];
- STEPPER[c]->set_speed(0);
- STEPPER[c]->move(this->home_direction[c], 10000000);
+ STEPPER[c]->move(this->home_direction[c], 10000000, 0);
}
}
if ( ( axes_to_move >> c ) & 1 ) {
inverted_dir = !this->home_direction[c];
this->feed_rate[c]= this->slow_rates[c];
- STEPPER[c]->set_speed(0);
- STEPPER[c]->move(inverted_dir, this->retract_mm[c]*STEPS_PER_MM(c));
+ STEPPER[c]->move(inverted_dir, this->retract_mm[c]*STEPS_PER_MM(c), 0);
}
}
for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
if ( ( axes_to_move >> c ) & 1 ) {
this->feed_rate[c]= this->slow_rates[c];
- STEPPER[c]->set_speed(0);
- STEPPER[c]->move(this->home_direction[c], 10000000);
+ STEPPER[c]->move(this->home_direction[c], 10000000, 0);
}
}
// move up or down depending on sign of trim, -ive is down away from home
if (this->trim_mm[c] < 0) inverted_dir = !inverted_dir;
this->feed_rate[c]= this->slow_rates[c];
- STEPPER[c]->set_speed(0);
- STEPPER[c]->move(inverted_dir, abs(round(this->trim_mm[c]*STEPS_PER_MM(c))));
+ STEPPER[c]->move(inverted_dir, abs(round(this->trim_mm[c]*STEPS_PER_MM(c))), 0);
}
}
{
this->status = MOVING_TO_ENDSTOP_FAST;
this->feed_rate[X_AXIS]= fast_rate;
- STEPPER[X_AXIS]->set_speed(0);
- STEPPER[X_AXIS]->move(dirx, 10000000);
+ STEPPER[X_AXIS]->move(dirx, 10000000, 0);
this->feed_rate[Y_AXIS]= fast_rate;
- STEPPER[Y_AXIS]->set_speed(0);
- STEPPER[Y_AXIS]->move(diry, 10000000);
+ STEPPER[Y_AXIS]->move(diry, 10000000, 0);
// wait for primary axis
this->wait_for_homed_corexy(home_axis);
// Move back a small distance
this->status = MOVING_BACK;
this->feed_rate[X_AXIS]= slow_rate;
- STEPPER[X_AXIS]->set_speed(0);
- STEPPER[X_AXIS]->move(!dirx, retract_steps);
+ STEPPER[X_AXIS]->move(!dirx, retract_steps, 0);
this->feed_rate[Y_AXIS]= slow_rate;
- STEPPER[Y_AXIS]->set_speed(0);
- STEPPER[Y_AXIS]->move(!diry, retract_steps);
+ STEPPER[Y_AXIS]->move(!diry, retract_steps, 0);
// wait until done
while ( STEPPER[X_AXIS]->is_moving() || STEPPER[Y_AXIS]->is_moving()) {
// Start moving the axes to the origin slowly
this->status = MOVING_TO_ENDSTOP_SLOW;
this->feed_rate[X_AXIS]= slow_rate;
- STEPPER[X_AXIS]->set_speed(0);
- STEPPER[X_AXIS]->move(dirx, 10000000);
+ STEPPER[X_AXIS]->move(dirx, 10000000, 0);
this->feed_rate[Y_AXIS]= slow_rate;
- STEPPER[Y_AXIS]->set_speed(0);
- STEPPER[Y_AXIS]->move(diry, 10000000);
+ STEPPER[Y_AXIS]->move(diry, 10000000, 0);
// wait for primary axis
this->wait_for_homed_corexy(home_axis);
// then move both X and Y until one hits the endstop
this->status = MOVING_TO_ENDSTOP_FAST;
+ // need to allow for more ground covered when moving diagonally
this->feed_rate[motor]= this->fast_rates[motor]*1.4142;
- STEPPER[motor]->set_speed(0); // need to allow for more ground covered when moving diagonally
- STEPPER[motor]->move(dir, 10000000);
+ STEPPER[motor]->move(dir, 10000000, 0);
// wait until either X or Y hits the endstop
bool running= true;
while (running) {
if (gcode->has_letter('F')) f = gcode->get_value('F');
if (gcode->has_letter('X')) {
x = gcode->get_value('X');
- STEPPER[X_AXIS]->set_speed(f);
- STEPPER[X_AXIS]->move(x<0, abs(x));
+ STEPPER[X_AXIS]->move(x<0, abs(x), f);
}
if (gcode->has_letter('Y')) {
y = gcode->get_value('Y');
- STEPPER[Y_AXIS]->set_speed(f);
- STEPPER[Y_AXIS]->move(y<0, abs(y));
+ STEPPER[Y_AXIS]->move(y<0, abs(y), f);
}
if (gcode->has_letter('Z')) {
z = gcode->get_value('Z');
- STEPPER[Z_AXIS]->set_speed(f);
- STEPPER[Z_AXIS]->move(z<0, abs(z));
+ STEPPER[Z_AXIS]->move(z<0, abs(z), f);
}
gcode->stream->printf("Moved X %d Y %d Z %d F %d steps\n", x, y, z, f);
gcode->mark_as_taken();
if( !STEPPER[c]->is_moving() ) continue;
uint32_t current_rate = STEPPER[c]->get_steps_per_second();
- uint32_t target_rate = int(floor(this->feed_rate[c]*STEPS_PER_MM(c)));
+ uint32_t target_rate = floorf(this->feed_rate[c]*STEPS_PER_MM(c));
float acc= (c==Z_AXIS) ? THEKERNEL->planner->get_z_acceleration() : THEKERNEL->planner->get_acceleration();
if( current_rate < target_rate ){
- uint32_t rate_increase = int(floor((acc/THEKERNEL->stepper->get_acceleration_ticks_per_second())*STEPS_PER_MM(c)));
+ uint32_t rate_increase = floorf((acc/THEKERNEL->stepper->get_acceleration_ticks_per_second())*STEPS_PER_MM(c));
current_rate = min( target_rate, current_rate + rate_increase );
}
if( current_rate > target_rate ){ current_rate = target_rate; }
// steps per second
- STEPPER[c]->set_speed(max(current_rate, THEKERNEL->stepper->get_minimum_steps_per_second()));
+ STEPPER[c]->set_speed(current_rate);
}
return 0;
#define PI 3.14159265358979F
-#define max(a,b) (((a) > (b)) ? (a) : (b))
/* The extruder module controls a filament extruder for 3D printing: http://en.wikipedia.org/wiki/Fused_deposition_modeling
* It can work in two modes : either the head does not move, and the extruder moves the filament at a specified speed ( SOLO mode here )
this->retracted = false;
this->volumetric_multiplier = 1.0F;
this->extruder_multiplier = 1.0F;
+ this->stepper_motor= nullptr;
memset(this->offset, 0, sizeof(this->offset));
}
+Extruder::~Extruder()
+{
+ delete stepper_motor;
+}
+
void Extruder::on_halt(void *arg)
{
if(arg == nullptr) {
void Extruder::on_module_loaded()
{
-
// Settings
this->on_config_reload(this);
+ // Start values
+ this->target_position = 0;
+ this->current_position = 0;
+ this->unstepped_distance = 0;
+ this->current_block = NULL;
+ this->mode = OFF;
+
// We work on the same Block as Stepper, so we need to know when it gets a new one and drops one
this->register_for_event(ON_BLOCK_BEGIN);
this->register_for_event(ON_BLOCK_END);
this->register_for_event(ON_SPEED_CHANGE);
this->register_for_event(ON_GET_PUBLIC_DATA);
- // Start values
- this->target_position = 0;
- this->current_position = 0;
- this->unstepped_distance = 0;
- this->current_block = NULL;
- this->mode = OFF;
-
// Update speed every *acceleration_ticks_per_second*
- // TODO: Make this an independent setting
THEKERNEL->slow_ticker->attach( THEKERNEL->stepper->get_acceleration_ticks_per_second() , this, &Extruder::acceleration_tick );
-
- // Stepper motor object for the extruder
- this->stepper_motor = THEKERNEL->step_ticker->add_stepper_motor( new StepperMotor(step_pin, dir_pin, en_pin) );
- this->stepper_motor->attach(this, &Extruder::stepper_motor_finished_move );
}
// Get config
this->steps_per_millimeter = THEKERNEL->config->value(extruder_steps_per_mm_checksum )->by_default(1)->as_number();
this->filament_diameter = THEKERNEL->config->value(extruder_filament_diameter_checksum )->by_default(0)->as_number();
this->acceleration = THEKERNEL->config->value(extruder_acceleration_checksum )->by_default(1000)->as_number();
- this->max_speed = THEKERNEL->config->value(extruder_max_speed_checksum )->by_default(1000)->as_number();
this->feed_rate = THEKERNEL->config->value(default_feed_rate_checksum )->by_default(1000)->as_number();
this->step_pin.from_string( THEKERNEL->config->value(extruder_step_pin_checksum )->by_default("nc" )->as_string())->as_output();
this->steps_per_millimeter = THEKERNEL->config->value(extruder_checksum, this->identifier, steps_per_mm_checksum )->by_default(1)->as_number();
this->filament_diameter = THEKERNEL->config->value(extruder_checksum, this->identifier, filament_diameter_checksum )->by_default(0)->as_number();
this->acceleration = THEKERNEL->config->value(extruder_checksum, this->identifier, acceleration_checksum )->by_default(1000)->as_number();
- this->max_speed = THEKERNEL->config->value(extruder_checksum, this->identifier, max_speed_checksum )->by_default(1000)->as_number();
this->feed_rate = THEKERNEL->config->value( default_feed_rate_checksum )->by_default(1000)->as_number();
this->step_pin.from_string( THEKERNEL->config->value(extruder_checksum, this->identifier, step_pin_checksum )->by_default("nc" )->as_string())->as_output();
if(filament_diameter > 0.01) {
this->volumetric_multiplier = 1.0F / (powf(this->filament_diameter / 2, 2) * PI);
}
+
+ // Stepper motor object for the extruder
+ this->stepper_motor = new StepperMotor(step_pin, dir_pin, en_pin);
+ this->stepper_motor->attach(this, &Extruder::stepper_motor_finished_move );
+ if( this->single_config ) {
+ this->stepper_motor->set_max_rate(THEKERNEL->config->value(extruder_max_speed_checksum)->by_default(1000)->as_number());
+ }else{
+ this->stepper_motor->set_max_rate(THEKERNEL->config->value(extruder_checksum, this->identifier, max_speed_checksum)->by_default(1000)->as_number());
+ }
}
void Extruder::on_get_public_data(void* argument){
if (gcode->has_letter('F')) {
feed_rate = gcode->get_value('F') / THEKERNEL->robot->get_seconds_per_minute();
- if (feed_rate > max_speed)
- feed_rate = max_speed;
+ if (feed_rate > stepper_motor->get_max_rate())
+ feed_rate = stepper_motor->get_max_rate();
}
}
}
-
}
// When a new block begins, either follow the robot, or step by ourselves ( or stay back and do nothing )
this->current_position += this->travel_distance ;
- int steps_to_step = abs(int(floor(this->steps_per_millimeter * (this->travel_distance + this->unstepped_distance) )));
+ int steps_to_step = abs(floorf(this->steps_per_millimeter * (this->travel_distance + this->unstepped_distance) ));
if ( this->travel_distance > 0 ) {
this->unstepped_distance += this->travel_distance - (steps_to_step / this->steps_per_millimeter); //catch any overflow
block->take();
this->current_block = block;
- this->stepper_motor->set_steps_per_second(0);
- this->stepper_motor->move( ( this->travel_distance > 0 ), steps_to_step);
+ this->stepper_motor->move( ( this->travel_distance > 0 ), steps_to_step, 0);
} else {
this->current_block = NULL;
this->current_position += this->travel_distance;
- int steps_to_step = abs(int(floor(this->steps_per_millimeter * (this->travel_distance + this->unstepped_distance) )));
+ int steps_to_step = abs(floorf(this->steps_per_millimeter * (this->travel_distance + this->unstepped_distance) ));
if ( this->travel_distance > 0 ) {
this->unstepped_distance += this->travel_distance - (steps_to_step / this->steps_per_millimeter); //catch any overflow
block->take();
this->current_block = block;
- this->stepper_motor->move( ( this->travel_distance > 0 ), steps_to_step );
- this->on_speed_change(0); // initialise speed in case we get called first
+ this->stepper_motor->move( ( this->travel_distance > 0 ), steps_to_step);
+ on_speed_change(this); // set initial speed
} else {
this->current_block = NULL;
}
}
// Called periodically to change the speed to match acceleration or to match the speed of the robot
+// Only used in SOLO mode
uint32_t Extruder::acceleration_tick(uint32_t dummy)
{
if(!this->enabled) return 0;
return 0;
}
+ if(!this->stepper_motor->is_moving()) return 0;
+
uint32_t current_rate = this->stepper_motor->get_steps_per_second();
- uint32_t target_rate = int(floor(this->feed_rate * this->steps_per_millimeter));
+ uint32_t target_rate = floorf(this->feed_rate * this->steps_per_millimeter);
if( current_rate < target_rate ) {
- uint32_t rate_increase = int(floor((this->acceleration / THEKERNEL->stepper->get_acceleration_ticks_per_second()) * this->steps_per_millimeter));
+ uint32_t rate_increase = floorf((this->acceleration / THEKERNEL->stepper->get_acceleration_ticks_per_second()) * this->steps_per_millimeter);
current_rate = min( target_rate, current_rate + rate_increase );
}
if( current_rate > target_rate ) {
}
// steps per second
- this->stepper_motor->set_speed(max(current_rate, THEKERNEL->stepper->get_minimum_steps_per_second()));
+ this->stepper_motor->set_speed(current_rate);
return 0;
}
if(!this->enabled) return;
// Avoid trying to work when we really shouldn't ( between blocks or re-entry )
- if( this->current_block == NULL || this->paused || this->mode != FOLLOW || this->stepper_motor->is_moving() != true ) {
+ if( this->current_block == NULL || this->paused || 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;
}
* or even : ( stepper steps per second ) * ( extruder steps / current block's steps )
*/
- this->stepper_motor->set_speed( max( ( THEKERNEL->stepper->get_trapezoid_adjusted_rate()) * ( (float)this->stepper_motor->get_steps_to_move() / (float)this->current_block->steps_event_count ), THEKERNEL->stepper->get_minimum_steps_per_second() ) );
-
+ //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);
+ this->stepper_motor->set_step_rate(THEKERNEL->stepper->get_trapezoid_adjusted_rate(), (float)this->current_block->steps_event_count);
}
// When the stepper has finished it's move
class Extruder : public Tool {
public:
Extruder(uint16_t config_identifier, bool single= false);
- virtual ~Extruder() {}
+ virtual ~Extruder();
void on_module_loaded();
void on_config_reload(void* argument);
};
float volumetric_multiplier;
- float feed_rate; //
- float max_speed;
+ float feed_rate; // mm/sec for SOLO moves only
float travel_ratio;
float travel_distance;
float maxz= this->max_z*2;
// move Z down
- STEPPER[Z_AXIS]->set_speed(0); // will be increased by acceleration tick
- STEPPER[Z_AXIS]->move(true, maxz * Z_STEPS_PER_MM); // always probes down, no more than 2*maxz
+ STEPPER[Z_AXIS]->move(true, maxz * Z_STEPS_PER_MM, 0); // always probes down, no more than 2*maxz
if(this->is_delta) {
// for delta need to move all three actuators
- STEPPER[X_AXIS]->set_speed(0);
- STEPPER[X_AXIS]->move(true, maxz * STEPS_PER_MM(X_AXIS));
- STEPPER[Y_AXIS]->set_speed(0);
- STEPPER[Y_AXIS]->move(true, maxz * STEPS_PER_MM(Y_AXIS));
+ STEPPER[X_AXIS]->move(true, maxz * STEPS_PER_MM(X_AXIS), 0);
+ STEPPER[Y_AXIS]->move(true, maxz * STEPS_PER_MM(Y_AXIS), 0);
}
// start acceration hrprocessing
bool dir= steps < 0;
steps= abs(steps);
- STEPPER[Z_AXIS]->set_speed(0); // will be increased by acceleration tick
- STEPPER[Z_AXIS]->move(dir, steps);
+ STEPPER[Z_AXIS]->move(dir, steps, 0);
if(this->is_delta) {
- STEPPER[X_AXIS]->set_speed(0);
- STEPPER[X_AXIS]->move(dir, steps);
- STEPPER[Y_AXIS]->set_speed(0);
- STEPPER[Y_AXIS]->move(dir, steps);
+ STEPPER[X_AXIS]->move(dir, steps, 0);
+ STEPPER[Y_AXIS]->move(dir, steps, 0);
}
this->running = true;
}
}
-#define max(a,b) (((a) > (b)) ? (a) : (b))
// Called periodically to change the speed to match acceleration
uint32_t ZProbe::acceleration_tick(uint32_t dummy)
{
void ZProbe::accelerate(int c)
{ uint32_t current_rate = STEPPER[c]->get_steps_per_second();
- uint32_t target_rate = int(floor(this->current_feedrate));
+ uint32_t target_rate = floorf(this->current_feedrate);
// Z may have a different acceleration to X and Y
float acc= (c==Z_AXIS) ? THEKERNEL->planner->get_z_acceleration() : THEKERNEL->planner->get_acceleration();
if( current_rate < target_rate ) {
- uint32_t rate_increase = int(floor((acc / THEKERNEL->stepper->get_acceleration_ticks_per_second()) * STEPS_PER_MM(c)));
+ uint32_t rate_increase = floorf((acc / THEKERNEL->stepper->get_acceleration_ticks_per_second()) * STEPS_PER_MM(c));
current_rate = min( target_rate, current_rate + rate_increase );
}
if( current_rate > target_rate ) {
}
// steps per second
- STEPPER[c]->set_speed(max(current_rate, THEKERNEL->stepper->get_minimum_steps_per_second()));
+ STEPPER[c]->set_speed(current_rate);
}
// issue a coordinated move directly to robot, and return when done
#include <string>
#include <math.h>
-#define max(a,b) (((a) > (b)) ? (a) : (b))
-
class AD5206 : public DigipotBase {
public:
AD5206(){
currents[channel]= -1;
return;
}
- current = min( max( current, 0.0L ), 2.0L );
+ current = min( max( current, 0.0F ), 2.0F );
char adresses[6] = { 0x05, 0x03, 0x01, 0x00, 0x02, 0x04 };
currents[channel] = current;
cs.set(0);
}
char current_to_wiper( float current ){
- return char(ceil(float((this->factor*current))));
+ return char(ceilf(float((this->factor*current))));
}
mbed::I2C* i2c;
#include "checksumm.h"
#include "PublicData.h"
#include "Gcode.h"
+//#include "StepTicker.h"
#include "modules/tools/temperaturecontrol/TemperatureControlPublicAccess.h"
#include "modules/robot/RobotPublicAccess.h"