Skip to content

Commit

Permalink
Marlin motion /working/ but sure not /right/.
Browse files Browse the repository at this point in the history
  • Loading branch information
Christopher Jansen committed Sep 3, 2011
1 parent a8e3b5c commit 4c497fd
Show file tree
Hide file tree
Showing 6 changed files with 137 additions and 38 deletions.
4 changes: 1 addition & 3 deletions GCode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -283,9 +283,7 @@ void GCode::do_m_code()
Host::Instance(source).write(' ');
#endif
Host::Instance(source).write("C: ");
#ifndef USE_MARLIN
MOTION.writePositionToHost(*this);
#endif
SETOBJ(writePositionToHost(*this));
// TODO
Host::Instance(source).endl();
state = DONE;
Expand Down
160 changes: 128 additions & 32 deletions Marlin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,9 +29,7 @@
#include <math.h>
#include <util/atomic.h>
#include <string.h>
#ifdef DEBUG_MOVE
#include "Host.h"
#endif

#define AXESLOOP(x) for(int x=0;x<NUM_AXIS;x++)

Expand Down Expand Up @@ -65,13 +63,20 @@ namespace Marlin

bool RELATIVEMOVES = false;

volatile bool needs_recompute = false;

#ifdef DEBUG_MOVE
void dump_block(block_t *b)
{
// Fields used by the bresenham algorithm for tracing the line
HOST.labelnum("SX:", b->steps[X_AXIS]);
AXESLOOP(ax)
{
HOST.write(ax);
HOST.write(':');
HOST.labelnum("S:", b->steps[ax]);
HOST.labelnum("SPD:", b->speed[ax]);
}
HOST.labelnum("SEC:", b->step_event_count);
HOST.labelnum("SPDX:", b->speed[X_AXIS]);
HOST.labelnum("NS:", b->nominal_speed);
HOST.labelnum("NR:", b->nominal_rate);
HOST.labelnum("MM:", b->millimeters);
Expand Down Expand Up @@ -311,7 +316,7 @@ namespace Marlin
// speed accordingly. Remember current->entry_factor equals the exit factor of
// the previous block.
if(previous->entry_speed < current->entry_speed) {
float max_entry_speed = max_allowable_speed(-current->acceleration, previous->entry_speed, previous->millimeters);
float max_entry_speed = max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters);
if (max_entry_speed < current->entry_speed) {
current->entry_speed = max_entry_speed;
}
Expand Down Expand Up @@ -457,10 +462,15 @@ namespace Marlin

for(int ax=0;ax<NUM_AXIS;ax++)
{
if(RELATIVEMOVES)
block->requestedposition[ax] = ((float)position[ax] / axis_steps_per_unit[X_AXIS]) + gcode[ax].getFloat();
if(gcode[ax].isUnused())
block->requestedposition[ax] = ((float)position[ax] / axis_steps_per_unit[ax]);
else
block->requestedposition[ax] = gcode[ax].getFloat();
{
if(RELATIVEMOVES)
block->requestedposition[ax] = ((float)position[ax] / axis_steps_per_unit[ax]) + gcode[ax].getFloat();
else
block->requestedposition[ax] = gcode[ax].getFloat();
}
}

if(gcode[F].isUnused())
Expand Down Expand Up @@ -489,24 +499,22 @@ void plan_buffer_line(block_t* block)
{

long target[NUM_AXIS];
block->step_event_count = 0;
AXESLOOP(ax)
{
// Calculate target position in absolute steps
target[ax] = lround(block->requestedposition[ax]*axis_steps_per_unit[ax]);
// Number of steps for each axis
block->steps[ax] = labs(target[ax]-position[ax]);
if(block->steps[ax] > block->step_event_count)
block->step_event_count = block->steps[ax];
}
block->step_event_count = max(block->steps[X_AXIS], max(block->steps[Y_AXIS], max(block->steps[Z_AXIS], block->steps[E_AXIS])));

// Bail if this is a zero-length block
if (block->step_event_count == 0) {
return;
};

#ifdef DEBUG_MOVE
HOST.write("Accepted Marlin move\n");
#endif

float delta_mm[NUM_AXIS];
AXESLOOP(ax)
{
Expand All @@ -529,16 +537,18 @@ void plan_buffer_line(block_t* block)
#endif

// Limit speed per axis
float speed_factor = 1;
float speed_factor=1;
float tmp_speed_factor;

AXESLOOP(ax)
{
if(abs(block->speed[ax]) > max_feedrate[ax])
{
tmp_speed_factor = max_feedrate[ax] / (float)abs(block->speed[ax]);

if(speed_factor > tmp_speed_factor)
speed_factor = tmp_speed_factor;
if(speed_factor > tmp_speed_factor)
speed_factor = tmp_speed_factor;
}
}
multiplier = multiplier * speed_factor;

Expand All @@ -563,8 +573,10 @@ void plan_buffer_line(block_t* block)
block->acceleration = axis_steps_per_sqr_second[X_AXIS];
// Limit acceleration per axis
AXESLOOP(ax)
{
if((block->acceleration * block->steps[ax] / block->step_event_count) > axis_steps_per_sqr_second[ax])
block->acceleration = axis_steps_per_sqr_second[ax];
block->acceleration = axis_steps_per_sqr_second[ax];
}

#ifdef DEBUG_MOVE
HOST.labelnum("tps:", travel_per_step);
Expand Down Expand Up @@ -611,8 +623,11 @@ void plan_buffer_line(block_t* block)
if(block->steps[ax] != 0) enable(ax);

// Update position
memcpy(position, target, sizeof(target)); // position[] = target[]
memcpy(block->endposition, position, sizeof(block->endposition));
AXESLOOP(ax)
{
position[ax] = target[ax];
block->endposition[ax] = target[ax];
}
}


Expand Down Expand Up @@ -725,6 +740,8 @@ void plan_buffer_line(block_t* block)
// The slope of acceleration is calculated with the leib ramp alghorithm.

void st_wake_up() {
if(needs_recompute)
return;
// TCNT1 = 0;
ENABLE_STEPPER_DRIVER_INTERRUPT();
#ifdef DEBUG_MOVE
Expand Down Expand Up @@ -785,7 +802,7 @@ void plan_buffer_line(block_t* block)
} // The busy-flag is used to avoid reentering this interrupt

busy = true;
//sei(); // Re enable interrupts (normally disabled while inside an interrupt handler)
sei(); // Re enable interrupts (normally disabled while inside an interrupt handler)

// If there is no current block, attempt to pop one from the buffer
if (current_block == NULL) {
Expand Down Expand Up @@ -848,14 +865,38 @@ void plan_buffer_line(block_t* block)
if (!current_block->axisdirections[ax])
{ // -direction
DIR_PINS[ax].setValue(INVERT_DIRS[ax]);
if(!MIN_PINS[ax].isNull() && MIN_PINS[ax].getValue() != ENDSTOP_INVERT)
step_events_completed = current_block->step_event_count;
if(!MIN_PINS[ax].isNull() && MIN_PINS[ax].getValue() != ENDSTOP_INVERT && current_block->steps[ax])
{
float sr = (float)current_block->step_event_count / (float)current_block->steps[ax];
current_block->endposition[ax] += (float)(current_block->step_event_count - step_events_completed) * sr;
current_block->steps[ax] = 0;
AXESLOOP(foo) if(current_block->steps[foo]) continue;
DISABLE_STEPPER_DRIVER_INTERRUPT();
current_block->busy = false;
current_block = NULL;
needs_recompute = true;
busy = false;
return;
//step_events_completed = current_block->step_event_count;
}
}
else // +direction
{
DIR_PINS[ax].setValue(!INVERT_DIRS[ax]);
if(!MAX_PINS[ax].isNull() && MAX_PINS[ax].getValue() != ENDSTOP_INVERT)
step_events_completed = current_block->step_event_count;
{
float sr = (float)current_block->step_event_count / (float)current_block->steps[ax];
current_block->endposition[ax] -= (float)(current_block->step_event_count - step_events_completed) * sr;
current_block->steps[ax] = 0;
AXESLOOP(foo) if(current_block->steps[foo]) continue;
DISABLE_STEPPER_DRIVER_INTERRUPT();
current_block->busy = false;
current_block = NULL;
needs_recompute = true;
busy = false;
return;
//step_events_completed = current_block->step_event_count;
}
}
}

Expand Down Expand Up @@ -917,11 +958,20 @@ void plan_buffer_line(block_t* block)
}
// If current block is finished, reset pointer
step_events_completed += 1;
if (step_events_completed >= current_block->step_event_count) {
if (step_events_completed >= current_block->step_event_count)
{
if(needs_recompute)
{
DISABLE_STEPPER_DRIVER_INTERRUPT();
current_block->busy = false;
current_block = NULL;
needs_recompute = true;
busy = false;
return;
}
current_block = NULL;
plan_discard_current_block();
}

busy=false;
}

Expand All @@ -931,7 +981,7 @@ void plan_buffer_line(block_t* block)
// Timer interrupt for E. e_steps is set in the main routine;
// Timer 0 is shared with millies
ISR(TIMER0_COMPA_vect)
{

// Critical section needed because Timer 1 interrupt has higher priority.
// The pin set functions are placed on trategic position to comply with the stepper driver timing.
STEP_PINS[E_AXIS].setValue(false);
Expand Down Expand Up @@ -978,10 +1028,9 @@ void plan_buffer_line(block_t* block)
Point& getCurrentPosition()
{
static Point p;
p[X] = position[X];
p[Y] = position[Y];
p[Z] = position[Z];
p[E] = position[E];
AXESLOOP(ax)
p[ax] = (float)position[ax] / axis_steps_per_unit[ax];

return p;
}
// Sets current position; doesn't cause a move, just updates the current position variables.
Expand All @@ -990,7 +1039,7 @@ void plan_buffer_line(block_t* block)
for(int ax=0;ax<NUM_AXIS;ax++)
{
if(!gcode[ax].isUnused())
position[ax] = gcode[ax].getFloat();
position[ax] = gcode[ax].getFloat() * axis_steps_per_unit[ax];
}
return;
}
Expand Down Expand Up @@ -1145,7 +1194,7 @@ void plan_buffer_line(block_t* block)
// Motors automatically enabled when used
void disableAllMotors() { AXESLOOP(ax) { disable(ax); } }
void wrapup(GCode& gcode) { return; }
void checkdisable(GCode& gcode) { return; }
void checkdisable(GCode& gcode) { check_axes_activity(); }

bool isBufferEmpty()
{
Expand All @@ -1172,6 +1221,53 @@ void plan_buffer_line(block_t* block)
#endif
}

void recompute_all_blocks() {
char block_index = block_buffer_tail;
block_t *b = NULL;

while(block_index != block_buffer_head) {
b = &block_buffer[block_index];
plan_buffer_line(b);
block_index = (block_index+1) & BLOCK_BUFFER_MASK;
}
}


void update()
{
if(needs_recompute)
{
block_t* b = plan_get_current_block();
if(b->busy)
return;
#ifdef DEBUG_MOVE
HOST.write("! RECALCULATE ALL !\n");
#endif
AXESLOOP(ax)
{
position[ax]=b->endposition[ax];
}
plan_discard_current_block();
recompute_all_blocks();
needs_recompute = false;
planner_recalculate();
st_wake_up();
}
check_axes_activity();
}

void writePositionToHost(GCode& gc)
{
AXESLOOP(ax)
{
Host::Instance(gc.source).write(ax > Z ? 'A' - Z - 1 + ax : 'X' + ax);
Host::Instance(gc.source).write(':');
Host::Instance(gc.source).write((float)position[ax] / axis_steps_per_unit[ax],0,4);
Host::Instance(gc.source).write(' ');
}
}




}; // namespace Marlin
3 changes: 3 additions & 0 deletions Marlin.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,7 @@ namespace Marlin
void st_wake_up();
bool add_buffer_line(GCode& gcode);
void plan_buffer_line(block_t *block);
void update();

bool isBufferFull();
bool isBufferEmpty();
Expand All @@ -75,6 +76,8 @@ namespace Marlin
Point& getCurrentPosition();
// Sets current position; doesn't cause a move, just updates the current position variables.
void setCurrentPosition(GCode &gcode);
// ...
void writePositionToHost(GCode& gc);
// Interpret movement data as absolute
void setAbsolute();
// Interpret movement data as relative
Expand Down
2 changes: 1 addition & 1 deletion config-common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
#else
#define GCODE_BUFSIZE 15
#define HOST_RECV_BUFSIZE 200
#define HOST_SEND_BUFSIZE 200
#define HOST_SEND_BUFSIZE 500
#endif

// Each source eats anough ram for 1 addtl gcode
Expand Down
2 changes: 1 addition & 1 deletion generic/ramps13ex.g
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ M302 X38 Y56 Z62 E24
M304 X3 Y14 Z18
M305 X2 Y15 Z19
M307 X0 Y0 Z1 E0
M308 X0 Y0 Z1 E0
M308 X0 Y0 Z0 E0
M200 X62.745 Y62.745 Z2267.718 E729.99
M201 X2000 Y2000 Z75 E5000
M203 X3000 Y3000 Z75 E5000
Expand Down
4 changes: 3 additions & 1 deletion main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,9 +70,11 @@ void mainloop()
BT.scan_input();
#endif

#ifdef USE_MARLIN
Marlin::update();
#endif
}


}


Expand Down

0 comments on commit 4c497fd

Please sign in to comment.