tons of changes, implementing 4D dda
This commit is contained in:
Michael Moon
parent
b184e71107
commit
1e6c74096e
|
|
@ -14,7 +14,7 @@
|
||||||
|
|
||||||
PROGRAM = mendel
|
PROGRAM = mendel
|
||||||
|
|
||||||
SOURCES = $(PROGRAM).c ringbuffer.c serial.c
|
SOURCES = $(PROGRAM).c ringbuffer.c serial.c dda.c gcode.c pinout.c
|
||||||
|
|
||||||
##############################################################################
|
##############################################################################
|
||||||
# #
|
# #
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,8 @@
|
||||||
#ifndef _ARDUINO_H
|
#ifndef _ARDUINO_H
|
||||||
#define _ARDUINO_H
|
#define _ARDUINO_H
|
||||||
|
|
||||||
|
#include <avr/io.h>
|
||||||
|
|
||||||
#define PIN_DIO0 PD0
|
#define PIN_DIO0 PD0
|
||||||
#define RPORT_DIO0 PIND
|
#define RPORT_DIO0 PIND
|
||||||
#define WPORT_DIO0 PORTD
|
#define WPORT_DIO0 PORTD
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,230 @@
|
||||||
|
#include "dda.h"
|
||||||
|
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
extern struct {
|
||||||
|
volatile int32_t X;
|
||||||
|
volatile int32_t Y;
|
||||||
|
volatile int32_t Z;
|
||||||
|
volatile int32_t E;
|
||||||
|
volatile int32_t F;
|
||||||
|
} current_position;
|
||||||
|
|
||||||
|
// courtesy of http://www.flipcode.com/archives/Fast_Approximate_Distance_Functions.shtml
|
||||||
|
uint32_t approx_distance( int32_t dx, int32_t dy )
|
||||||
|
{
|
||||||
|
uint32_t min, max;
|
||||||
|
|
||||||
|
if ( dx < 0 ) dx = -dx;
|
||||||
|
if ( dy < 0 ) dy = -dy;
|
||||||
|
|
||||||
|
if ( dx < dy )
|
||||||
|
{
|
||||||
|
min = dx;
|
||||||
|
max = dy;
|
||||||
|
} else {
|
||||||
|
min = dy;
|
||||||
|
max = dx;
|
||||||
|
}
|
||||||
|
|
||||||
|
// coefficients equivalent to ( 123/128 * max ) and ( 51/128 * min )
|
||||||
|
return ((( max << 8 ) + ( max << 3 ) - ( max << 4 ) - ( max << 1 ) +
|
||||||
|
( min << 7 ) - ( min << 5 ) + ( min << 3 ) - ( min << 1 )) >> 8 );
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
CREATE
|
||||||
|
*/
|
||||||
|
|
||||||
|
void dda_create(GCODE_COMMAND *cmd, DDA *dda) {
|
||||||
|
static TARGET startpoint = { 0, 0, 0, 0, 0 };
|
||||||
|
|
||||||
|
// we start at the previous endpoint
|
||||||
|
memcpy(&dda->currentpoint, &startpoint, sizeof(TARGET));
|
||||||
|
// we end at the passed command's endpoint
|
||||||
|
memcpy(&dda->endpoint, &cmd->target, sizeof(TARGET));
|
||||||
|
|
||||||
|
dda->x_delta = dda->endpoint.X - startpoint.X;
|
||||||
|
dda->y_delta = dda->endpoint.Y - startpoint.Y;
|
||||||
|
// always relative
|
||||||
|
dda->e_delta = dda->endpoint.E;
|
||||||
|
// always absolute
|
||||||
|
dda->f_delta = dda->endpoint.F - startpoint.F;
|
||||||
|
|
||||||
|
|
||||||
|
dda->distance = approx_distance(dda->x_delta, dda->y_delta);
|
||||||
|
|
||||||
|
if (dda->distance < 2)
|
||||||
|
dda->distance = dda->e_delta;
|
||||||
|
if (dda->distance < 2)
|
||||||
|
dda->distance = dda->f_delta;
|
||||||
|
|
||||||
|
dda->total_steps = dda->x_delta;
|
||||||
|
if (dda->y_delta > dda->total_steps)
|
||||||
|
dda->total_steps = dda->y_delta;
|
||||||
|
if (dda->e_delta > dda->total_steps)
|
||||||
|
dda->total_steps = dda->e_delta;
|
||||||
|
if (dda->f_delta > dda->total_steps)
|
||||||
|
dda->total_steps = dda->f_delta;
|
||||||
|
|
||||||
|
if (dda->total_steps == 0)
|
||||||
|
dda->nullmove = 1;
|
||||||
|
|
||||||
|
// MM = sqrt(X^2 + Y^2)
|
||||||
|
// STEPS = max(X * STEPS_PER_MM_X, Y * STEPS_PER_MM_Y)
|
||||||
|
// DURATION = MM / MM_PER_MIN * 60 SEC_PER_MIN * 1000000 US_PER_SEC
|
||||||
|
// US/STEP = DURATION / STEPS
|
||||||
|
// intF = sqrt(X^2 + Y^2) / max(X * STEPS_PER_MM_X, Y * STEPS_PER_MM_Y)
|
||||||
|
|
||||||
|
// dda->endpoint.F = distance / total_steps;
|
||||||
|
|
||||||
|
if (dda->f_delta > dda->total_steps) {
|
||||||
|
// TODO: rescale F
|
||||||
|
dda->f_scale = dda->f_delta / dda->total_steps;
|
||||||
|
if (dda->f_scale > 3) {
|
||||||
|
dda->f_delta /= dda->f_scale;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
dda->f_scale = 1;
|
||||||
|
dda->total_steps = dda->f_delta;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
dda->x_direction = (dda->endpoint.X > startpoint.X)?1:0;
|
||||||
|
dda->y_direction = (dda->endpoint.Y > startpoint.Y)?1:0;
|
||||||
|
dda->e_direction = (dda->endpoint.E > startpoint.E)?1:0;
|
||||||
|
dda->f_direction = (dda->endpoint.F > startpoint.F)?1:0;
|
||||||
|
|
||||||
|
dda->x_counter = dda->y_counter = dda->e_counter = dda->f_counter
|
||||||
|
= -(dda->total_steps >> 1);
|
||||||
|
|
||||||
|
// next dda starts where we finish
|
||||||
|
memcpy(&startpoint, &dda->endpoint, sizeof(TARGET));
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
START
|
||||||
|
*/
|
||||||
|
|
||||||
|
void dda_start(DDA *dda) {
|
||||||
|
x_direction(dda->x_direction);
|
||||||
|
y_direction(dda->y_direction);
|
||||||
|
z_direction(dda->z_direction);
|
||||||
|
e_direction(dda->e_direction);
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
CAN STEP
|
||||||
|
*/
|
||||||
|
|
||||||
|
uint8_t can_step(uint8_t min, uint8_t max, int32_t current, int32_t target, uint8_t dir) {
|
||||||
|
if (target == current)
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
if (min && !dir)
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
if (max && dir)
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
return 255;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
STEP
|
||||||
|
*/
|
||||||
|
|
||||||
|
void dda_step(DDA *dda) {
|
||||||
|
uint8_t step_option = 0;
|
||||||
|
#define X_CAN_STEP 1
|
||||||
|
#define Y_CAN_STEP 2
|
||||||
|
#define Z_CAN_STEP 4
|
||||||
|
#define E_CAN_STEP 8
|
||||||
|
#define F_CAN_STEP 16
|
||||||
|
#define REAL_MOVE 32
|
||||||
|
|
||||||
|
do {
|
||||||
|
step_option |= can_step(x_min(), x_max(), current_position.X, dda->endpoint.X, dda->x_direction) & X_CAN_STEP;
|
||||||
|
step_option |= can_step(y_min(), y_max(), current_position.Y, dda->endpoint.Y, dda->y_direction) & Y_CAN_STEP;
|
||||||
|
step_option |= can_step(z_min(), z_max(), current_position.Z, dda->endpoint.Z, dda->z_direction) & Z_CAN_STEP;
|
||||||
|
step_option |= can_step(-1, -1, current_position.E, dda->endpoint.E, dda->e_direction) & E_CAN_STEP;
|
||||||
|
step_option |= can_step(-1, -1, current_position.F, dda->endpoint.F, dda->f_direction) & F_CAN_STEP;
|
||||||
|
|
||||||
|
if (step_option & X_CAN_STEP) {
|
||||||
|
dda->x_counter += dda->x_delta;
|
||||||
|
if (dda->x_counter > 0) {
|
||||||
|
step_option |= REAL_MOVE;
|
||||||
|
|
||||||
|
// do X step
|
||||||
|
dda->x_counter -= dda->total_steps;
|
||||||
|
|
||||||
|
if (dda->x_direction)
|
||||||
|
current_position.X++;
|
||||||
|
else
|
||||||
|
current_position.X--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (step_option & Y_CAN_STEP) {
|
||||||
|
dda->y_counter += dda->y_delta;
|
||||||
|
if (dda->y_counter > 0) {
|
||||||
|
step_option |= REAL_MOVE;
|
||||||
|
|
||||||
|
// do Y step
|
||||||
|
dda->y_counter -= dda->total_steps;
|
||||||
|
|
||||||
|
if (dda->y_direction)
|
||||||
|
current_position.Y++;
|
||||||
|
else
|
||||||
|
current_position.Y--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (step_option & Z_CAN_STEP) {
|
||||||
|
dda->z_counter += dda->z_delta;
|
||||||
|
if (dda->z_counter > 0) {
|
||||||
|
step_option |= REAL_MOVE;
|
||||||
|
|
||||||
|
// do Z step
|
||||||
|
dda->z_counter -= dda->total_steps;
|
||||||
|
|
||||||
|
if (dda->z_direction)
|
||||||
|
current_position.Z++;
|
||||||
|
else
|
||||||
|
current_position.Z--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (step_option & E_CAN_STEP) {
|
||||||
|
dda->e_counter += dda->e_delta;
|
||||||
|
if (dda->e_counter > 0) {
|
||||||
|
step_option |= REAL_MOVE;
|
||||||
|
|
||||||
|
// do E step
|
||||||
|
dda->e_counter -= dda->total_steps;
|
||||||
|
|
||||||
|
if (dda->e_direction)
|
||||||
|
current_position.E++;
|
||||||
|
else
|
||||||
|
current_position.E--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (step_option & F_CAN_STEP) {
|
||||||
|
dda->f_counter += dda->f_delta;
|
||||||
|
if (dda->f_counter > 0) {
|
||||||
|
// do F step
|
||||||
|
dda->f_counter -= dda->total_steps;
|
||||||
|
|
||||||
|
if (dda->f_direction)
|
||||||
|
current_position.F += dda->f_scale;
|
||||||
|
else
|
||||||
|
current_position.F -= dda->f_scale;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} while (((step_option & REAL_MOVE) == 0) && (step_option & F_CAN_STEP));
|
||||||
|
|
||||||
|
if (step_option & REAL_MOVE) {
|
||||||
|
setTimer(dda->distance * (60000000 / current_position.F / dda->total_steps));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,47 @@
|
||||||
|
#ifndef _DDA_H
|
||||||
|
#define _DDA_H
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#include "target.h"
|
||||||
|
#include "pinout.h"
|
||||||
|
#include "gcode.h"
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
TARGET currentpoint;
|
||||||
|
TARGET endpoint;
|
||||||
|
|
||||||
|
uint8_t steep :1;
|
||||||
|
uint8_t swap :1;
|
||||||
|
uint8_t x_direction :1;
|
||||||
|
uint8_t y_direction :1;
|
||||||
|
uint8_t z_direction :1;
|
||||||
|
uint8_t e_direction :1;
|
||||||
|
uint8_t f_direction :1;
|
||||||
|
uint8_t nullmove :1;
|
||||||
|
|
||||||
|
int8_t x_delta;
|
||||||
|
int8_t y_delta;
|
||||||
|
int8_t z_delta;
|
||||||
|
int8_t e_delta;
|
||||||
|
int8_t f_delta;
|
||||||
|
|
||||||
|
int32_t x_counter;
|
||||||
|
int32_t y_counter;
|
||||||
|
int32_t z_counter;
|
||||||
|
int32_t e_counter;
|
||||||
|
int32_t f_counter;
|
||||||
|
|
||||||
|
uint32_t total_steps;
|
||||||
|
|
||||||
|
uint16_t f_scale;
|
||||||
|
uint32_t distance;
|
||||||
|
} DDA;
|
||||||
|
|
||||||
|
uint32_t approx_distance( int32_t dx, int32_t dy );
|
||||||
|
|
||||||
|
void dda_create(GCODE_COMMAND *cmd, DDA *dda);
|
||||||
|
void dda_start(DDA *dda);
|
||||||
|
void dda_step(DDA *dda);
|
||||||
|
|
||||||
|
#endif /* _DDA_H */
|
||||||
|
|
@ -0,0 +1,194 @@
|
||||||
|
#include "gcode.h"
|
||||||
|
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#include "machine.h"
|
||||||
|
#include "dda.h"
|
||||||
|
|
||||||
|
extern uint8_t mb_head;
|
||||||
|
extern uint8_t mb_tail;
|
||||||
|
extern DDA movebuffer[16];
|
||||||
|
extern uint8_t option_bitfield;
|
||||||
|
|
||||||
|
#define PI 3.1415926535
|
||||||
|
|
||||||
|
/*
|
||||||
|
utility functions
|
||||||
|
*/
|
||||||
|
|
||||||
|
uint8_t indexof(uint8_t c, char *string) {
|
||||||
|
uint8_t i;
|
||||||
|
for (i = 0;string[i];i++) {
|
||||||
|
if (c == string[i])
|
||||||
|
return i;
|
||||||
|
}
|
||||||
|
return 255;
|
||||||
|
}
|
||||||
|
|
||||||
|
float manexp_to_float(uint32_t mantissa, uint8_t exp) {
|
||||||
|
float v = mantissa;
|
||||||
|
|
||||||
|
if (exp == 2)
|
||||||
|
v /= 10;
|
||||||
|
else if (exp == 3)
|
||||||
|
v /= 100;
|
||||||
|
else if (exp == 4)
|
||||||
|
v /= 1000;
|
||||||
|
else if (exp == 5)
|
||||||
|
v /= 10000;
|
||||||
|
else if (exp == 6)
|
||||||
|
v /= 100000;
|
||||||
|
|
||||||
|
return v;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
public functions
|
||||||
|
*/
|
||||||
|
|
||||||
|
void scan_char(uint8_t c) {
|
||||||
|
static uint8_t last_field = 0;
|
||||||
|
static uint32_t mantissa = 0;
|
||||||
|
static uint8_t exp = 0;
|
||||||
|
static GCODE_COMMAND next_target = { 0, 0, 0, 0, { 0, 0, 0, 0, 0 } };
|
||||||
|
|
||||||
|
// uppercase
|
||||||
|
if (c >= 'a' && c <= 'z')
|
||||||
|
c &= ~32;
|
||||||
|
|
||||||
|
// process field
|
||||||
|
if (indexof(c, "GMXYZEF\n") != 255) {
|
||||||
|
if (last_field) {
|
||||||
|
switch (last_field) {
|
||||||
|
case 'G':
|
||||||
|
next_target.G = mantissa;
|
||||||
|
break;
|
||||||
|
case 'M':
|
||||||
|
next_target.M = mantissa;
|
||||||
|
break;
|
||||||
|
case 'X':
|
||||||
|
next_target.target.X = manexp_to_float(mantissa, exp) * STEPS_PER_MM_X;
|
||||||
|
break;
|
||||||
|
case 'Y':
|
||||||
|
next_target.target.Y = manexp_to_float(mantissa, exp) * STEPS_PER_MM_Y;
|
||||||
|
break;
|
||||||
|
case 'Z':
|
||||||
|
next_target.target.Z = manexp_to_float(mantissa, exp) * STEPS_PER_MM_Z;
|
||||||
|
break;
|
||||||
|
case 'E':
|
||||||
|
next_target.target.E = manexp_to_float(mantissa, exp) * STEPS_PER_MM_E;
|
||||||
|
break;
|
||||||
|
case 'F':
|
||||||
|
// TODO: calculate end speed in microseconds per step from millimeters per minute
|
||||||
|
// MM = sqrt(X^2 + Y^2)
|
||||||
|
// STEPS = max(X * STEPS_PER_MM_X, Y * STEPS_PER_MM_Y)
|
||||||
|
// DURATION = MM / MM_PER_MIN * 60 SEC_PER_MIN * 1000000 US_PER_SEC
|
||||||
|
// US/STEP = DURATION / STEPS
|
||||||
|
// intF = sqrt(X^2 + Y^2) / max(X * STEPS_PER_MM_X, Y * STEPS_PER_MM_Y)
|
||||||
|
next_target.target.F = manexp_to_float(mantissa, exp) * STEPS_PER_MM_F;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
mantissa = 0;
|
||||||
|
exp = 0;
|
||||||
|
}
|
||||||
|
last_field = c;
|
||||||
|
switch (c) {
|
||||||
|
case 'G':
|
||||||
|
next_target.seen |= SEEN_G;
|
||||||
|
break;
|
||||||
|
case 'M':
|
||||||
|
next_target.seen |= SEEN_M;
|
||||||
|
break;
|
||||||
|
case 'X':
|
||||||
|
next_target.seen |= SEEN_X;
|
||||||
|
break;
|
||||||
|
case 'Y':
|
||||||
|
next_target.seen |= SEEN_Y;
|
||||||
|
break;
|
||||||
|
case 'Z':
|
||||||
|
next_target.seen |= SEEN_Z;
|
||||||
|
break;
|
||||||
|
case 'E':
|
||||||
|
next_target.seen |= SEEN_E;
|
||||||
|
break;
|
||||||
|
case 'F':
|
||||||
|
next_target.seen |= SEEN_F;
|
||||||
|
break;
|
||||||
|
case '\n':
|
||||||
|
// process
|
||||||
|
process_gcode_command(&next_target);
|
||||||
|
|
||||||
|
// save options
|
||||||
|
option_bitfield = next_target.option;
|
||||||
|
|
||||||
|
// reset variables
|
||||||
|
last_field = 0;
|
||||||
|
memset(&next_target, 0, sizeof(GCODE_COMMAND));
|
||||||
|
next_target.option = option_bitfield;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// process digits
|
||||||
|
else if (c == '-')
|
||||||
|
exp |= 0x80;
|
||||||
|
else if ((c == '.') && ((exp & 0x7F) == 0))
|
||||||
|
exp |= 1;
|
||||||
|
else if (c >= '0' && c <= '9') {
|
||||||
|
mantissa = (mantissa * 10) + (c - '0');
|
||||||
|
if (exp & 0x7F)
|
||||||
|
exp++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void process_gcode_command(GCODE_COMMAND *gcmd) {
|
||||||
|
uint8_t do_move;
|
||||||
|
if (gcmd->seen & SEEN_G) {
|
||||||
|
switch (gcmd->G) {
|
||||||
|
// G0 - rapid, unsynchronised motion
|
||||||
|
case 0:
|
||||||
|
gcmd->option &= ~OPTION_SYNCHRONISE;
|
||||||
|
do_move = 1;
|
||||||
|
break;
|
||||||
|
// G30 - go home via point
|
||||||
|
case 30:
|
||||||
|
gcmd->option |= OPTION_HOME_WHEN_COMPLETE;
|
||||||
|
// G1 - synchronised motion
|
||||||
|
case 1:
|
||||||
|
gcmd->option |= OPTION_SYNCHRONISE;
|
||||||
|
do_move = 1;
|
||||||
|
break;
|
||||||
|
// G2 - Arc Clockwise
|
||||||
|
// G3 - Arc Counter-clockwise
|
||||||
|
// G4 - Dwell
|
||||||
|
// G20 - inches as units
|
||||||
|
// G21 - mm as units
|
||||||
|
// G28 - go home
|
||||||
|
case 28:
|
||||||
|
gcmd->option &= ~OPTION_SYNCHRONISE;
|
||||||
|
do_move = 1;
|
||||||
|
break;
|
||||||
|
// G90 - absolute positioning
|
||||||
|
case 90:
|
||||||
|
gcmd->option &= ~OPTION_RELATIVE;
|
||||||
|
break;
|
||||||
|
// G91 - relative positioning
|
||||||
|
case 91:
|
||||||
|
gcmd->option |= OPTION_RELATIVE;
|
||||||
|
break;
|
||||||
|
// G92 - set home
|
||||||
|
case 92:
|
||||||
|
break;
|
||||||
|
// TODO: spit an error
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (gcmd->seen & SEEN_M) {
|
||||||
|
switch (gcmd->M) {
|
||||||
|
// TODO: spit an error
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (do_move) {
|
||||||
|
dda_create(gcmd, movebuffer);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,30 @@
|
||||||
|
#ifndef _GCODE_H
|
||||||
|
#define _GCODE_H
|
||||||
|
|
||||||
|
#include "target.h"
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
uint16_t seen;
|
||||||
|
#define SEEN_G 1
|
||||||
|
#define SEEN_M 2
|
||||||
|
#define SEEN_X 4
|
||||||
|
#define SEEN_Y 8
|
||||||
|
#define SEEN_Z 16
|
||||||
|
#define SEEN_E 32
|
||||||
|
#define SEEN_F 64
|
||||||
|
|
||||||
|
uint8_t option;
|
||||||
|
#define OPTION_RELATIVE 1
|
||||||
|
#define OPTION_SYNCHRONISE 2
|
||||||
|
#define OPTION_HOME_WHEN_COMPLETE 4
|
||||||
|
#define OPTION_UNIT_INCHES 8
|
||||||
|
|
||||||
|
uint8_t G;
|
||||||
|
uint8_t M;
|
||||||
|
TARGET target;
|
||||||
|
} GCODE_COMMAND;
|
||||||
|
|
||||||
|
void scan_char(uint8_t c);
|
||||||
|
void process_gcode_command(GCODE_COMMAND *gcmd);
|
||||||
|
|
||||||
|
#endif /* _GCODE_H */
|
||||||
|
|
@ -0,0 +1,45 @@
|
||||||
|
#ifndef _MACHINE_H
|
||||||
|
#define _MACHINE_H
|
||||||
|
|
||||||
|
/*
|
||||||
|
machine variables
|
||||||
|
*/
|
||||||
|
#define AXIS_COUNT 5
|
||||||
|
#define AXIS_HOMING_COUNT 3
|
||||||
|
|
||||||
|
/*
|
||||||
|
axis calculations, adjust as necessary
|
||||||
|
*/
|
||||||
|
|
||||||
|
#define XY_STEPS_PER_REV 3200
|
||||||
|
#define XY_COG_CIRCUMFERENCE (4.77 * 16)
|
||||||
|
|
||||||
|
#define EXTRUDER_STEPS_PER_REV 3200
|
||||||
|
#define EXTRUDER_SHAFT_RADIUS 5
|
||||||
|
#define EXTRUDER_INLET_DIAMETER 3
|
||||||
|
#define EXTRUDER_NOZZLE_DIAMETER 0.8
|
||||||
|
|
||||||
|
|
||||||
|
#define STEPS_PER_MM_X (XY_STEPS_PER_REV * XY_COG_CIRCUMFERENCE)
|
||||||
|
#define STEPS_PER_MM_Y (XY_STEPS_PER_REV * XY_COG_CIRCUMFERENCE)
|
||||||
|
#define STEPS_PER_MM_Z (3200)
|
||||||
|
#define STEPS_PER_MM_E (EXTRUDER_STEPS_PER_REV * EXTRUDER_SHAFT_RADIUS * PI * EXTRUDER_INLET_DIAMETER / EXTRUDER_NOZZLE_DIAMETER)
|
||||||
|
|
||||||
|
#define STEPS_PER_MM_PER_S_F (3200)
|
||||||
|
|
||||||
|
#define STEPS_PER_MM_F STEPS_PER_MM_PER_S_F
|
||||||
|
|
||||||
|
/*
|
||||||
|
F is sent in units of millimeters per second
|
||||||
|
and implemented as microseconds per step
|
||||||
|
|
||||||
|
MM/S * steps/mm * 1000000us/s = steps per microsecond
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
#define FAST_MM_PER_SEC 40
|
||||||
|
#define SLOW_MM_PER_SEC 20
|
||||||
|
#define ACCEL 10
|
||||||
|
|
||||||
|
|
||||||
|
#endif /* _MACHINE_H */
|
||||||
206
mendel/mendel.c
206
mendel/mendel.c
|
|
@ -1,80 +1,30 @@
|
||||||
#include <stddef.h>
|
#include <stddef.h>
|
||||||
// #include <stdio.h>
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
#include <avr/io.h>
|
#include <avr/io.h>
|
||||||
#include <avr/interrupt.h>
|
#include <avr/interrupt.h>
|
||||||
|
|
||||||
#include "serial.h"
|
#include "serial.h"
|
||||||
|
|
||||||
/*
|
#include "dda.h"
|
||||||
machine variables
|
#include "gcode.h"
|
||||||
*/
|
|
||||||
#define AXIS_COUNT 5
|
|
||||||
#define AXIS_HOMING_COUNT 3
|
|
||||||
|
|
||||||
// steps per rev * mm per tooth * teeth per rev
|
#include "machine.h"
|
||||||
#define STEPS_PER_MM (3200 * 4.77 * 16)
|
|
||||||
#define STEPS_PER_IN (STEPS_PER_MM * 25.4)
|
|
||||||
|
|
||||||
#define MAX_MM_PER_SEC 40
|
uint8_t mb_head = 0;
|
||||||
#define MAX_IMMED_MM_PER_SEC 20
|
uint8_t mb_tail = 0;
|
||||||
#define ACCEL 10
|
DDA movebuffer[16];
|
||||||
|
|
||||||
/*
|
|
||||||
state machine variables
|
|
||||||
*/
|
|
||||||
|
|
||||||
uint8_t linebuffer[64];
|
|
||||||
|
|
||||||
uint8_t state = 0;
|
|
||||||
#define STATE_WAIT_FOR_COMMAND 0
|
|
||||||
#define STATE_WAIT_FOR_COMMAND_DIGIT 1
|
|
||||||
#define STATE_WAIT_FOR_DATA 2
|
|
||||||
#define STATE_WAIT_FOR_DATA_DIGIT 3
|
|
||||||
uint8_t command;
|
|
||||||
uint8_t command_digit;
|
|
||||||
uint8_t axis;
|
|
||||||
|
|
||||||
uint8_t option_bitfield;
|
uint8_t option_bitfield;
|
||||||
#define OPTION_RELATIVE 1
|
|
||||||
#define OPTION_SYNCHRONISE 2
|
|
||||||
#define OPTION_HOME_WHEN_COMPLETE 4
|
|
||||||
#define OPTION_UNIT_INCHES 8
|
|
||||||
|
|
||||||
// volatile uint32_t xpos = 0;
|
struct {
|
||||||
// volatile uint32_t ypos = 0;
|
volatile int32_t X;
|
||||||
// volatile uint32_t zpos = 0;
|
volatile int32_t Y;
|
||||||
// volatile uint32_t edelta = 0;
|
volatile int32_t Z;
|
||||||
//
|
volatile int32_t E;
|
||||||
// uint32_t xtarget = 0;
|
volatile int32_t F;
|
||||||
// uint32_t ytarget = 0;
|
} current_position = { 0, 0, 0, 0, 0 };
|
||||||
// uint32_t ztarget = 0;
|
|
||||||
//
|
|
||||||
// uint16_t xspeed = 0;
|
|
||||||
// uint16_t yspeed = 0;
|
|
||||||
// uint16_t zspeed = 0;
|
|
||||||
|
|
||||||
struct axis {
|
|
||||||
volatile uint32_t pos;
|
|
||||||
int32_t target;
|
|
||||||
uint32_t newtarget_mantissa;
|
|
||||||
uint8_t newtarget_opt;
|
|
||||||
#define newtarget_opt_sign 0x80
|
|
||||||
#define newtarget_opt_set 0x40
|
|
||||||
#define newtarget_opt_exp 0x3F
|
|
||||||
uint16_t speed;
|
|
||||||
} axes[AXIS_COUNT];
|
|
||||||
|
|
||||||
uint8_t axis_char_to_id(uint8_t c) {
|
|
||||||
if (c >= 'X' && c <= 'Z')
|
|
||||||
return c - 'X';
|
|
||||||
if (c == 'E')
|
|
||||||
return 3;
|
|
||||||
if (c == 'F')
|
|
||||||
return 4;
|
|
||||||
return 255;
|
|
||||||
}
|
|
||||||
|
|
||||||
int main (void)
|
int main (void)
|
||||||
{
|
{
|
||||||
|
|
@ -88,131 +38,7 @@ int main (void)
|
||||||
{
|
{
|
||||||
for (;serial_rxchars() == 0;);
|
for (;serial_rxchars() == 0;);
|
||||||
uint8_t c = serial_popchar();
|
uint8_t c = serial_popchar();
|
||||||
if (c >= 'a' && c <= 'z')
|
|
||||||
c &= ~32;
|
scan_char(c);
|
||||||
// preprocess
|
|
||||||
switch (state) {
|
|
||||||
case STATE_WAIT_FOR_COMMAND_DIGIT:
|
|
||||||
if (c == 'G' || c == 'M')
|
|
||||||
state = STATE_WAIT_FOR_COMMAND;
|
|
||||||
break;
|
|
||||||
case STATE_WAIT_FOR_DATA_DIGIT:
|
|
||||||
if (axis_char_to_id(c) < 255)
|
|
||||||
state = STATE_WAIT_FOR_DATA;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
// actuate
|
|
||||||
switch (state) {
|
|
||||||
case STATE_WAIT_FOR_COMMAND:
|
|
||||||
if (c == 'G' || c == 'M') {
|
|
||||||
command = c;
|
|
||||||
command_digit = 0;
|
|
||||||
state = STATE_WAIT_FOR_COMMAND_DIGIT;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case STATE_WAIT_FOR_COMMAND_DIGIT:
|
|
||||||
if (c >= '0' && c <= '9') {
|
|
||||||
command_digit = (command_digit * 10) + (c - '0');
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
state = STATE_WAIT_FOR_DATA;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case STATE_WAIT_FOR_DATA:
|
|
||||||
if (axis_char_to_id(c) < 255) {
|
|
||||||
axis = axis_char_to_id(c);
|
|
||||||
axes[axis].newtarget_mantissa = 0;
|
|
||||||
axes[axis].newtarget_opt = 0;
|
|
||||||
state = STATE_WAIT_FOR_DATA_DIGIT;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case STATE_WAIT_FOR_DATA_DIGIT:
|
|
||||||
if (c == '-') {
|
|
||||||
axes[axis].newtarget_opt |= newtarget_opt_sign;
|
|
||||||
}
|
|
||||||
if (c >= '0' && c <= '9') {
|
|
||||||
axes[axis].newtarget_mantissa = (axes[axis].newtarget_mantissa * 10) + (c - '0');
|
|
||||||
if (axes[axis].newtarget_opt & newtarget_opt_exp)
|
|
||||||
axes[axis].newtarget_opt++;
|
|
||||||
}
|
|
||||||
if (c == '.') {
|
|
||||||
axes[axis].newtarget_opt |= 1;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
if (c == 13) {
|
|
||||||
if (command == 'G') {
|
|
||||||
uint8_t i;
|
|
||||||
switch (command_digit) {
|
|
||||||
// G30 - go home via point
|
|
||||||
case 30:
|
|
||||||
option_bitfield |= OPTION_HOME_WHEN_COMPLETE;
|
|
||||||
// G0 - rapid, unsynchronised motion
|
|
||||||
case 0:
|
|
||||||
option_bitfield &= ~OPTION_SYNCHRONISE;
|
|
||||||
break;
|
|
||||||
// G1 - synchronised motion
|
|
||||||
case 1:
|
|
||||||
option_bitfield |= OPTION_SYNCHRONISE;
|
|
||||||
break;
|
|
||||||
// G2 - Arc Clockwise
|
|
||||||
// G3 - Arc Counter-clockwise
|
|
||||||
// G4 - Dwell
|
|
||||||
// G20 - inches as units
|
|
||||||
// G21 - mm as units
|
|
||||||
// G28 - go home
|
|
||||||
case 28:
|
|
||||||
for (i = 0; i < AXIS_HOMING_COUNT; i++) {
|
|
||||||
option_bitfield &= ~OPTION_SYNCHRONISE;
|
|
||||||
axes[i].target = 0;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
// G90 - absolute positioning
|
|
||||||
case 90:
|
|
||||||
option_bitfield &= ~OPTION_RELATIVE;
|
|
||||||
break;
|
|
||||||
// G91 - relative positioning
|
|
||||||
case 91:
|
|
||||||
option_bitfield |= OPTION_RELATIVE;
|
|
||||||
break;
|
|
||||||
// G92 - set home
|
|
||||||
case 92:
|
|
||||||
for (i = 0; i < AXIS_HOMING_COUNT; i++) {
|
|
||||||
axes[i].pos = axes[i].target = 0;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
// TODO: spit an error
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else if (command == 'M') {
|
|
||||||
switch (command_digit) {
|
|
||||||
// TODO: spit an error
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// update axes;
|
|
||||||
uint8_t i;
|
|
||||||
for (i = 0; i < AXIS_COUNT; i++) {
|
|
||||||
if (axes[i].newtarget_opt & newtarget_opt_set) {
|
|
||||||
float n = axes[i].newtarget_mantissa;
|
|
||||||
uint8_t exp = axes[i].newtarget_opt & newtarget_opt_exp;
|
|
||||||
if (axes[i].newtarget_opt & newtarget_opt_sign)
|
|
||||||
n = -n;
|
|
||||||
if (exp == 1)
|
|
||||||
n /= 10;
|
|
||||||
if (exp == 2)
|
|
||||||
n /= 100;
|
|
||||||
if (exp == 3)
|
|
||||||
n /= 1000;
|
|
||||||
if (exp == 4)
|
|
||||||
n /= 10000;
|
|
||||||
if (exp == 5)
|
|
||||||
n /= 100000;
|
|
||||||
if (option_bitfield & OPTION_UNIT_INCHES)
|
|
||||||
axes[i].target = n * STEPS_PER_IN;
|
|
||||||
else
|
|
||||||
axes[i].target = n * STEPS_PER_MM;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,48 @@
|
||||||
|
#include "pinout.h"
|
||||||
|
|
||||||
|
void delayMicrosecondsInterruptible(uint16_t us)
|
||||||
|
{
|
||||||
|
// for a one-microsecond delay, simply return. the overhead
|
||||||
|
// of the function call yields a delay of approximately 1 1/8 us.
|
||||||
|
if (--us == 0)
|
||||||
|
return;
|
||||||
|
|
||||||
|
// the following loop takes a quarter of a microsecond (4 cycles)
|
||||||
|
// per iteration, so execute it four times for each microsecond of
|
||||||
|
// delay requested.
|
||||||
|
us <<= 2;
|
||||||
|
|
||||||
|
// account for the time taken in the preceeding commands.
|
||||||
|
us -= 2;
|
||||||
|
|
||||||
|
// busy wait
|
||||||
|
__asm__ __volatile__ ("1: sbiw %0,1" "\n\t" // 2 cycles
|
||||||
|
"brne 1b" :
|
||||||
|
"=w" (us) :
|
||||||
|
"0" (us) // 2 cycles
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
void x_step() {
|
||||||
|
WRITE(DIO0, 1);
|
||||||
|
delayMicrosecondsInterruptible(5);
|
||||||
|
WRITE(DIO0, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void y_step() {
|
||||||
|
WRITE(DIO4, 1);
|
||||||
|
delayMicrosecondsInterruptible(5);
|
||||||
|
WRITE(DIO4, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void z_step() {
|
||||||
|
WRITE(DIO8, 1);
|
||||||
|
delayMicrosecondsInterruptible(5);
|
||||||
|
WRITE(DIO8, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void e_step() {
|
||||||
|
WRITE(DIO12, 1);
|
||||||
|
delayMicrosecondsInterruptible(5);
|
||||||
|
WRITE(DIO12, 0);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,80 @@
|
||||||
|
#ifndef _PINOUT_H
|
||||||
|
#define _PINOUT_H
|
||||||
|
|
||||||
|
#include "arduino.h"
|
||||||
|
|
||||||
|
#ifndef MASK
|
||||||
|
#define MASK(PIN) (1 << PIN)
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define _PIN(P) #P
|
||||||
|
|
||||||
|
#define READ(IO) (RPORT_ ## IO & MASK(PIN_ ## IO))
|
||||||
|
#define WRITE(IO, v) if (v) { WPORT_ ## IO |= MASK(PIN_ ## IO); } else { WPORT_ ## IO &= ~MASK(PIN_ ## IO); }
|
||||||
|
#define SET_INPUT(IO) (DDR_ ## IO |= MASK(PIN_ ## IO))
|
||||||
|
#define SET_OUTPUT(IO) (DDR_ ## IO &= ~MASK(PIN ## IO))
|
||||||
|
|
||||||
|
// #define X_STEP_PIN DIO0
|
||||||
|
// #define X_DIR_PIN DIO1
|
||||||
|
// #define X_MIN_MIN DIO2
|
||||||
|
// #define X_MAX_PIN DIO3
|
||||||
|
|
||||||
|
// #define Y_STEP_PIN DIO4
|
||||||
|
// #define Y_DIR_PIN DIO5
|
||||||
|
// #define Y_MIN_MIN DIO6
|
||||||
|
// #define Y_MAX_PIN DIO7
|
||||||
|
|
||||||
|
// #define Z_STEP_PIN DIO8
|
||||||
|
// #define Z_DIR_PIN DIO9
|
||||||
|
// #define Z_MIN_MIN DIO10
|
||||||
|
// #define Z_MAX_PIN DIO11
|
||||||
|
|
||||||
|
// #define E_STEP_PIN DIO12
|
||||||
|
// #define E_DIR_PIN DIO13
|
||||||
|
|
||||||
|
void x_step(void);
|
||||||
|
void y_step(void);
|
||||||
|
void z_step(void);
|
||||||
|
void e_step(void);
|
||||||
|
|
||||||
|
inline void x_direction(uint8_t dir) {
|
||||||
|
WRITE(DIO1, dir);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline uint8_t x_min(void) {
|
||||||
|
return READ(DIO2);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline uint8_t x_max(void) {
|
||||||
|
return READ(DIO3);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void y_direction(uint8_t dir) {
|
||||||
|
WRITE(DIO5, dir);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline uint8_t y_min(void) {
|
||||||
|
return READ(DIO6);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline uint8_t y_max(void) {
|
||||||
|
return READ(DIO7);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void z_direction(uint8_t dir) {
|
||||||
|
WRITE(DIO9, dir);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline uint8_t z_min(void) {
|
||||||
|
return READ(DIO10);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline uint8_t z_max(void) {
|
||||||
|
return READ(DIO11);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void e_direction(uint8_t dir) {
|
||||||
|
WRITE(DIO13, dir);
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* _PINOUT_H */
|
||||||
|
|
@ -0,0 +1,14 @@
|
||||||
|
#ifndef _TARGET_H
|
||||||
|
#define _TARGET_H
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
int32_t X;
|
||||||
|
int32_t Y;
|
||||||
|
int32_t Z;
|
||||||
|
uint32_t E;
|
||||||
|
uint32_t F;
|
||||||
|
} TARGET;
|
||||||
|
|
||||||
|
#endif /* _TARGET_H */
|
||||||
Loading…
Reference in New Issue